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abstract A robot device observes the external world by a stereo vision system (1) 
and outputs stereo data (D1) as a distance image, the stereo data (D1) being 
three-dimensional distance information calculated by a parallax of both eyes. A plane 



detector (2) detects planes from the distance image to recognize the planes present 
in the environment. From plane data (D2), a stair recognizer (3) extracts a plane which 
the robot can climb up and down, recognizes a stair from that plane, and outputs stair 
data (D4). A stair climb-up/-down controller (4) outputs a behavior control command 
(D5) for realizing the movement for climbing up and down the stair by using the stair 
data (D4). This enables a mobile body itself to obtain information on a stair to 
autonomously perform the movement of stair climb up and down. 

Dispositif robot qui observe le monde exterieur a I'aide d'un systeme de vision stereo 
(1) et produit des donnees stereo (D1) en tant qu'image de distance, lesdites donnees 
stereo (D1) etant des informations de distance en trois dimensions calculees par une 
parallaxe des deux yeux. Un detecteur (2) de plans detecte des plans a partir de 
Timage de distance pour reconnaTtre les plans presents dans I'environnement. A partir 
des donnees de plans (D2), un reconnaisseur (3) d'escalier extrait un plan que le robot 
peut monter et descendre, reconnatt un escalier dans ce plan et produit des donnees 
d'escalier (D4). Un dispositif de commande de montee / descente (4) des marches 
produit une instruction de commande de comportement (D5) permettant au robot 
d'effectuer le mouvement de montee et de descente des marches £ I'aide des 
donnees d'escalier (D4). Cela permet a un corps mobile d'obtenir des informations sur 
un escalier pour effectuer de maniere autonome le mouvement de montee et de 
descente dudit escalier. 
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DESCRIPTION 

Robot Apparatus, Method of controlling the motion thereof, and 
Locomotion Mechanism 

Technical Field 

The present invention relates to a robot apparatus, a locomotion mechanism 
used in the robot apparatus and including locomotion means such as legs etc. to 
enable the robot apparatus to climb a stairway plural steps and to move otherwise, 
and a method of controlling the stair climbing of the robot apparatus. 

This application claims the priority of the Japanese Patent Application No. 
2004-077214 filed on March 17, 2004, the entirety of which is incorporated by 
reference herein. 

Background Art 

Heretofore, multiple techniques have been proposed for enabling a robot 
apparatus with a locomotion mechanism including locomotion means such as 
wheels and the like to move in a stepped environment and up and down on stairs. 

The above conventional techniques include, for example, a method of 
enabling a robot apparatus to move up and down on an object on the basis of 
already-known stair information including object node data intended for 
recognition of shape features of the object as shown in FIG. 1 (as disclosed in the 
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Japanese Patent No. 3176701) and a method of enabling a robot apparatus to move 
up and down on stairs on the basis of stair data detected by many tactile sensors 
602 attached to an entire foot base 601 in the form of a matrix and covered with a 
protective membrane 603 as shown in FIG. 2 (as disclosed in the Japanese Patent 
No. 3278467). The techniques also include a method of enabling the stair 
climbing of a robot apparatus by providing an infrared sensor on either side of each 
foot base of the robot apparatus and a landmark tape on either side of the stair as 
shown in FIG. 3 (as disclosed in the Japanese Patent No. 3330710). In the above 
third method (disclosed in the Japanese Patent No. 3330710), there are used a 
plurality of photosensors 682 provided on either side of each of foot bases 622R 
and 622L of a bipedal walking (or biped locomotion) robot apparatus, and straight 
landmarkers 680 each formed from a paint which will well absorb light, such as 
black paint, and having a predetermined width as shown in FIG. 3A. Outputs 
from the photosensors 682 in pair are compared with each other to detect a 
direction of the feet in relation to the landmarkers 680. With such foot bases 
622R and 622L and landmarkers 680, it is possible to recognize the position on a 
stair 690 as shown in FIG. 3B. 

However, since the robot apparatus is controlled to move up and down on 
stairs on the basis of the already -known stair information, the technique disclosed 
in the Japanese Patent No. 3176701 is not usable to control the movement of the 
robot apparatus in any unknown environment. Therefore, it is difficult to apply 
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this technique to any autonomous robot apparatus etc. for example. Also, the 
techniques disclosed in the Japanese Patent Nos. 3278467 and 3330710 are such 
that the stair climbing of the robot apparatus is controlled based on the stair 
information detected by the plurality of photosensors provided on or on either side 
of the foot base. Thus, the stair information cannot be acquired before the robot 
apparatus places the feet on the tread of a stair. Since no details of any stairs can 
be viewed from a far position, these techniques can only be utilized in case it can 
be predicted that stairs exist ahead of the robot apparatus. 

Disclosure of the Invention 

It is therefore an object of the present invention to overcome the 
above-mentioned drawbacks of the related art by providing a robot apparatus, a 
locomotion mechanism that enables autonomous stair climbing of the robot 
apparatus by acquiring information on stairs on which the robot apparatus is going 
to move up or down, and a method of controlling the motion of the robot apparatus. 

The above object can be attained by providing a robot apparatus enabled by 
a locomotive means to move, the apparatus including according to the present 
invention: 

a plane segmenting/extracting means for detecting a plane or a plurality of 
planes included in an environment surrounding the apparatus from 
three-dimensional distance data and outputting the detected plane or planes as plane 
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information; 

a stair recognizing means for recognizing, based on the plane information, a 
stair having a plane on which the apparatus can move and outputting stair 
information including information on the tread and riser of the stair; and 

a stair-climb controlling means for controlling the stair climbing of the 
apparatus by judging, based on the stair information, whether the apparatus can 
climb the stair and having the apparatus autonomously position itself in relation to 
the stair tread when the stair is determined to be climbable. 

In the above locomotion robot apparatus including legs, for example, as the 
locomotion means according to the present invention, it is judged, based on the 
tread information such as the dimensions, positions, etc. of a tread, whether the foot 
bases of the apparatus can be placed on the tread, and it is judged, based on the 
riser information indicating a riser height, whether the apparatus can move from 
one to another tread over the riser height. It is when the result of judgment is that 
the apparatus can move so that the apparatus can autonomously position itself to 
ascend or descend the stairs. 

Also, the above robot apparatus may include a distance measuring means for 
acquiring the three-dimensional distance data, and thus it can autonomously climb 
stairs, if any detected. 

Also, the above stair recognizing means may include a stair extracting means 
for detecting, based on the supplied plane information, a stair having a plane on 
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which the apparatus can move and outputting stair information not yet merged, and 
a stair merging means for statistically processing a plurality of yeMo-be^nerged 
stair information supplied from the stair extracting means and which are temporally 
different from each other and outputting merged stair information. By these 
means, even a robot apparatus having a narrow field of view or not capable of 
recognizing stairs well with one time of stair extraction can recognize the stairs 
accurately in a wide range on the basis of the merged stair information resulted 
from the time-serial and statistic merging of such stair information. 

Further, the above stair extracting means can recognize the dimensions and 
spatial positions of a tread on the basis of the plane information and output the 
resultant tread information as the yeMo-be^nerged stair information, and the stair 
merging means can merge, when a group of two or more treads overlapping each 
other in a range larger than a predetermined threshold and whose difference in 
height from each other is smaller than a predetermined threshold has been detected 
from temporally continuous tread information, the tread information together into a 
single piece of tread information including tread information on all the treads in the 
tread group. Thus, a wide range of stair recognition is assured by merging the 
tread information to cover all the treads selected for merging of the information on 
them. 

Furthermore, the stair recognizing means can recognize the dimensions and 
spatial position of a tread on the basis of the plane information, and take them as 
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the tread information. The tread information may include information on a front 
edge as the outermost edge, and information on a back edge as the innermost edge, 
of the tread in at least the moving direction. Since the stair recognizing means 
recognizes the front and back edges of the tread, it can recognize the tread 
accurately even in the case of a spiral stair, for example, to thereby enable the 
apparatus to climb the stairs. 

Also, the tread information may include right- and left-margin information 
indicative of marginal regions adjacent to the right and left sides, respectively, of a 
safety zone defined by the front and back edges and in which it is estimated that it 
is highly probable that the apparatus can move, reference-point information 
indicative of the center of gravity of a region which is estimated, based on the plane 
information, to be a tread, three-dimensional coordinate information on a group of 
points forming together a plane which is to be a tread, etc. The stair climbing can 
be controlled accurately on the basis of these pieces of tread information. 

Further, the stair recognizing means can extract the boundary of a plane on 
the basis of the plane information to calculate a polygon, and calculate the tread 
information from the polygon. For example, in case the field of view of the robot 
apparatus is narrow, when three-dimensional distance data is highly reliable, the 
polygon may be a convex polygonal region circumscribing the plane boundary 
extracted based on the plane information and thus may include an actually detected 
plane. On the other hand, if the distance data contains many noises, the polygon 
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may be a convex polygonal region inscribing the plane boundary extracted on the 
basis of the plane information and be included in the actually detected plane, so that 
the noise part can be cut off to accurately detect the tread. 

Furthermore, the apparatus having moved to a predetermined position 
opposite to the back edge of a tread on which it is moving (will be referred to as 
"present tread" hereunder) can be controlled to climb the stairs. For example, in 
the case of stairs each having a tread whose front and back edges overlap each 
other, such as low-riser stairs, the apparatus can be controlled to climb the stairs by 
moving it toward the back edge. Similarly, the apparatus may be controlled to 
climb the stairs by moving toward the front edge. 

Also, if the back edge of a present tread cannot be recognized, the 
stair-climb controlling means can control the apparatus to climb the stairs after the 
apparatus has moved to a predetermined position opposite to the front edge of a 
next tread to which the apparatus is to climb. For example, when the stair-climb 
controlling means detects a stair when the apparatus is moving on a floor surface, 
the back edge of the floor surface does not overlap the front edge of a first tread of 
the stair as the case may be. In such a case, the stair-climb controlling means can 
control the apparatus to climb the stair by moving it toward the front edge of the 
next tread to which the apparatus is to climb. 

Also, when the stair-climb controlling means detects a next tread to which 
the apparatus is to move, it can control the apparatus to climb the stairs by 
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performing a series of operations for moving to a predetermined position opposite 
to the tread to which the apparatus is to move. Each time the apparatus moves to a 
new tread, the stair-climb controlling means can control the apparatus to climb the 
tread by making a search for the tread, align itself to the tread and approach to the 
tread (search-align-approach operation). 

Further, if the stair-climb controlling means cannot extract a next tread and 
subsequent treads with the apparatus standing in a present position or on a present 
tread, it can find, based on already acquired stair information, a next tread to which 
the apparatus is to move. By acquiring stair information on a several-steps higher 
or lower tread in advance, the stair-climb controlling means can control the 
apparatus to climb the stairs even if the apparatus is not constructed to acquire 
information on its nearest surroundings and namely its field of view is narrow. 

Also, after the apparatus has moved to a predetermined position opposite to 
the back edge of a present tread, the stair-climb controlling means can detect a next 
tread to which the apparatus is to move, have the apparatus move to a 
predetermined position opposite to the front edge of the tread and have the 
apparatus climb to the tread. Thus, based on the data on the front and back edges, 
the stair-climb controlling means can control the apparatus to climb even a spiral 
stair in which both the edges are not parallel to each other. 

Further, the stair-climb controlling means can control the stair climbing of 
the apparatus on the basis of parameters defining a position of the locomotion 
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means in relation to a tread. The parameters can be determined on the basis of a 
height of foot lifting or lowering, for example, of the leg. The stair-climb 
controlling means may include a parameter selecting means for selecting parameter 
values for an intended stair-ascending or -descending motion. Thus, the 
stair-climb controlling means can control both the stair-ascending and -descending 
motions in the same manner just by changing the values of parameters for 
stair-ascending or -descending motion, whichever is intended. 

Also, the plane segmenting/extracting means includes a line fitting means for 
fitting line segments (will be referred to as "line" hereunder wherever appropriate) 
for each group of distance data points estimated to be in one plane in a 
three-dimensional space, and a planar region growing means for extracting a 
plurality of lines estimated to be in one plane from the group of lines fitted by the 
line fitting means and calculating a plane from the extracted plurality of lines. 

The line fitting means can fit lines adaptively to the distribution of the 
distance data points. It extracts lines based on the fact that three-dimensional 
distance data in one plane are in one straight line. Since the distribution of 
distance data points is varied under the influence of noises or the like at this time, 
the line fitting means fits lines adaptively to the distribution of distance data 
(adaptive line fitting), to thereby enable an accurate line fitting robustly against the 
noises and determine a plane from the fitted plurality of lines by the scan line 
grouping method. Thus, a plane can accurately be extracted without taking a 
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plurality of normally existing planes as a single plane or taking a normally existing 
plane as a plurality of planes, which would otherwise be caused by the influence of 
the noises or the like. 

Further, the line fitting means can extract a group of distance data points 
estimated to be in one plane on the basis of the distance between the distance data 
points, and re-estimate, based on the distribution of the distance data points in the 
distance data point group, whether the distance data point group exists in one plane. 
It can accurately fit lines by extracting a distance data point group once on the basis 
of a distance between the distance data points in the three-dimensional space and 
estimating, based on the data point distribution, whether the distance data point 
group lies in one plane. 

Furthermore, the line fitting means can extract a first line from the group of 
distance data points estimated to lie in one plane, take a distance data point in the 
group, whose distance from the first line is longest, as a point of interest, extract a 
second line from the distance data point group in case the distance is smaller than a 
predetermined threshold, judge whether a larger number of distance data points 
than a predetermined number exist continuously at one side of the second line, and 
segment the distance data point group by the point of interest in case the larger 
number of distance data points than the predetermined number exist continuously. 
For example, the line fitting means takes a line connecting end points of an 
extracted data point group as the first line, and generates a second line by the 
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least-squares method, for example, in case there exists a point whose distance from 
the first line is long. In case there exists a plurality of data points continuously at 
one side of the second line, the line fitting means can assume that the data point 
group is in the zig-zag shape, for example, in relation to the line and thus determine 
that the extracted data point group is biased, and segment the data point group by 
the point of interest or the like. 

Also, the planar region grouping means can select more than one line 
estimated to be in one plane and calculate a reference plane, search lines estimated 
to be in the plane in which the reference plane lies as grouping lines from the group 
of lines, update the reference plane with the grouping lines and repeat the grouping 
of the region of the reference plane, and output the updated plane as an updated one. 
Thus, the planar region grouping means can make the planar region growing and 
plane updating with the lines estimated to lie in one plane. 

Further, the planar region grouping means may includes a plane 
recalculating means for recalculating a plane from the group of distance data points 
except for ones whose distance from the updated plane is larger than a 
predetermined threshold, if any, existing in the distance data point group in the 
updated plane. Since the updated plane is taken as a plane in which all lines are 
uniformly distributed, it is possible to have a result of extraction less influenced by 
noises or the like by recalculating a plane from the group of distance data points 
except for ones largely different from the plane in which all the lines are uniformly 

11 



S05P0303WO00 

distributed. 

Furthermore, the planar region grouping means can estimate, based on a 
difference between a lines -determined plane and reference plane, whether the lines 
lie coplanar with the reference plane. Namely, it is possible to extract a plane 
more accurately by judging, based on a root mean square residual of the plane 
equation, for example, whether the extraction has been influenced by noises or the 
lines are not coplanar with the reference plane. 

Also the above object can be attained by providing a method of controlling 
the motion of a robot apparatus enabled by a locomotive means to move, the 
method including, according to the present invention, the steps of: 

detecting a plane or a plurality of planes included in an environment 
surrounding the apparatus from three-dimensional distance data and outputting the 
detected plane or planes as plane information; 

recognizing, based on the plane information, a stair having a plane on which 
the apparatus can move and outputting stair information including information on 
the tread and riser of the stair; and 

controlling the stair climbing of the apparatus by judging, based on the stair 
information, whether the apparatus can climb the stair and having the apparatus 
autonomously position itself in relation to the stair tread when the stair is 
determined to be climbable. 

Also the above object can be attained by providing a locomotion mechanism 
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movable by locomotion means, the mechanism including according to the present 
invention: 

a plane segmenting/extracting means for detecting a plane or a plurality of 
planes included in an environment surrounding the apparatus from 
three-dimensional distance data and outputting the detected plane or planes as plane 
information; 

a stair recognizing means for recognizing, based on the plane information, a 
stair having a plane on which the apparatus can move and outputting stair 
information including information on the tread and riser of the stair; and 

a stair-climb controlling means for controlling the stair climbing of the 
apparatus by judging, based on the stair information, whether the apparatus can 
climb the stair and having the apparatus autonomously position itself in relation to 
the stair tread when the stair is determined to be climbable. 

In the robot apparatus including legs and the like as locomotion means and 
locomotion mechanism used in the robot apparatus as above, it is possible to judge, 
based on tread information on the dimensions and position of a stair tread, whether 
the foot base of the robot apparatus has such a size that the foot base can be placed 
on the tread, judge, based on riser information indicative of a step height, whether 
the robot apparatus can move to the tread of such a height, and climb the stairs by 
autonomously positioning the robot apparatus in case it has been determined that 
the robot apparatus can move to the tread. 

13 
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These objects and other objects, features and advantages of the present 
invention will become more apparent from the following detailed description of the 
preferred embodiments of the present invention when taken in conjunction with the 
accompanying drawings. 

Brief Description of the Drawings 

FIG. 1 explains an example of the conventional stair-climb controlling 
techniques; 

FIG. 2 explains another example of the conventional stair-climb controlling 
techniques; 

FIGS. 3 A and 3B explain still another example of the conventional 
stair-climb controlling techniques; 

FIG. 4 is a perspective view of a robot apparatus according to the present 
invention; 

FIG. 5 schematically illustrates the degrees of freedom of the joints of the 
robot apparatus; 

FIG. 6 schematically illustrates the control system employed in the robot 
apparatus; 

FIG. 7 is a functional block diagram of a system in which the robot 
apparatus makes operations from acquisition of stereo data to stair climbing; 

FIG. 8A schematically illustrates the robot apparatus taking a picture of its 
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surroundings and FIG. 8B shows the dimensions of the foot bases of the robot 
apparatus; 

FIG. 9 explains the stair extraction, FIG. 9A shows the front view of the stair, 
FIG. 9B shows the side elevation of the stair, and FIG. 9C shows the perspective 
view of the stair; 

FIG. 10 explains another embodiment of the stair extraction, FIG. 10 A 
shows the front view of the stair, FIG. 10B shows the side elevation of the stair, 
and FIG. 10C shows the perspective view of the stair; 

FIG. 1 1 shows examples of the results of the extraction of the stairs in FIG. 9, 
FIG. 11A schematically illustrates the segmentation of the image of the stair in FIG. 
9, and FIGS. 1 IB to 1 ID show the three-dimensional distance data acquired from 
images in FIG. 1 1 A; 

FIG. 12 shows other examples of the results of the extraction of the stairs in 
FIG. 10, FIG. 12A schematically illustrates the segmentation of the image of the 
stair in FIG. 10, and FIGS. 12B to 12D show the three-dimensional distance data 
acquired from images in FIG. 12A; 

FIG. 13A schematically illustrates the segmentation of the image of the stair, 
FIG 13B shows the result of the extraction of the four plane regions A to D 
acquired from the three-dimensional distance data of FIG. 13 A, showing another 
example of the result of the extraction of the stair; 

FIG. 14 shows functional blocks of the stair recognition unit; 
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FIG. 15 shows a flow of operations made in the stair extraction; 

FIG. 16 schematically illustrates a convex polygon; 

FIG. 17 schematically illustrates the Melkman's algorithm; 

FIG. 18 is a schematic illustration for explaining how to determine a polygon 
by the Sklansky's algorithm; 

FIG. 19 is a schematic illustration for explaining a problem taking place with 
a non-polygonal stair, in which FIG. 19A shows an input plane and FIG. 19B 
shows the result of representation of the non-polygonal stair in a polygonal form by 
a convex hull. 

FIG. 20 is a schematic illustration for explaining a method of determining a 
polygon including an input plane by smoothing, in which FIG. 20A shows the input 
plane, FIG. 20B shows a polygon smoothed by removing discontinuous gaps from 
a polygon representing the input plane and FIG. 20C shows a polygon further 
smoothed by fitting line segments of the polygon shown in FIG. 20B. 

FIG. 21 shows an example of the program for determining a polygon 
including an input plane by smoothing with the close-gaps and fit-line-segments 
operations . 

FIGS. 22A and 22B show schematic illustration for explaining how to 
calculate stair parameters; 

FIG. 23 is a schematic illustration for explaining a finally recognized tread 
and stair parameters; 
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FIGS. 24 A and 24B schematically illustrate stairs; 
FIG. 25 shows a flow of operations made in stair-information merging; 
FIG. 26 is a schematic illustration for explaining the merging of stair data 
overlapping each other; 

FIG. 27 explains an align mode of operation; 

FIG. 28 is a schematic illustration for explaining an approach mode of 
operation; 

FIG. 29 shows a flow of operations made in a stair climb mode of operation; 

FIG. 30 shows a flow of operations made in the search, align and approach 
modes of operation (search-align-approach); 

FIG. 31 shows a flow of operations made in the stair climb mode; 

FIG. 32 schematically illustrates a tread the robot apparatus has recognized 
or is going to recognize; 

FIG. 33 shows a flow of operations made in the stair climb mode; 

FIG. 34 also shows a flow of operations made in the stair climb mode; 

FIG. 35A explains the relation between a tread the robot apparatus has 
recognized and the foot base and FIG. 35B shows the measurement of each unit; 

FIG. 36 are traced drawings of the robot apparatus ascending stairs; 

FIG. 37 are traced drawings of the robot apparatus descending the stairs; 

FIG. 38 shows a geometric relation between a single step and the foot bases 
of the robot apparatus; 
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FIG. 39 shows a geometric relation between a single concavity and the foot 
bases of the robot apparatus; 

FIG. 40 shows functional blocks of the plane segmentation/extractor unit in 
this variant; 

FIG. 41 explains a robot apparatus including a means for imparting a texture; 

FIG. 42 explains how to extract planes by the scan line grouping method in a 
variant of the aforementioned embodiment; 

FIG. 43 shows a flow of operations made in the plane 
segmentation/extraction by the scan line grouping method; 

FIG. 44 shows a flow of operations made in the plane segmentation/extractor 
unit in the variant; 

FIG. 45 shows how distance data points are distributed, in which FIG. 45A 
shows the data being distributed in the zig-zag shape in relation to a line and FIG. 
45B shows the data being distributed evenly near a line under the influence of 
noises or the like; 

FIG. 46 shows a flow of operations made in the zig-zag shape discrimination 
method in the variant; 

■ 

FIG. 47 shows an exemplary program of the zig-zag shape discrimination; 
FIG. 48 is a block diagram of the zig-zag shape discriminator; 
FIG. 49 is a schematic illustration for explaining the region growing in the 
variant; 
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FIG. 50 shows a flow of operations made in seed-region search and region 
growing in the region grouping unit in the variant; 

FIG. 51 schematically illustrates examples equal in distance between an end 
point and straight line to each other but different in root mean square residual rms 
of the plane equation from each other, in which FIG. 51 A shows an example in 
which the line is off the plane under the influence of noises or the like and FIG. 
5 IB shows an example in which there exists another plane in which the line is; 

FIG. 52 shows selection of a seed region; 

FIG. 53 shows the region growing; 

FIG. 54 A is a schematic illustration of a floor surface viewed from the robot 
apparatus being in upright position, FIG. 54B shows three-dimensional distance 
data with the vertical axis taken as x, horizontal axis as y and the density of each 
data point taken as z, and straight lines extracted from a group of data points lying 
in one plane or line-directional pixel train by the line fitting process, and FIG. 54C 
shows a planar region determined by the region growing from the group of straight 
lines shown in FIG. 54B; 

FIG. 55 explains the difference in result of performance between the plane 
segmentation/extraction method according to the variant and conventional plane 
segmentation/extraction method when a step is placed on a flat surface, in which 
FIG. 55A schematically illustrates an image being viewed, FIG. 55B shows 
experimental conditions, FIG. 55C shows the results of plane 
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segmentation/extraction by the plane segmentation/extraction method according to 
the variant, and FIG. 55D shows the results of plane segmentation/extraction by the 
conventional plane segmentation/extraction method; and 

FIG. 56 A schematically illustrates an image of a floor surface, and FIGS. 
56B and 56C show lines extracted by the line fitting method according to the 
variant and by the conventional line fitting method, respectively, from the 
horizontal and vertical distance data point trains, respectively, included in the 
three-dimensional distance data acquired by taking a picture of the floor surface 
shown in FIG. 56A. 

■ 

Best Mode for Carrying out the Invention 

* 

The embodiment which will be illustrated and explained hereunder is an 
application of the present invention to an autonomous locomotion robot apparatus 
including a step recognition unit to recognize a step such as a stair or the like 
existing in the surrounding environment. 

The robot apparatus according to this embodiment of the present invention is 
capable of recognizing stairs on the basis of a plurality of planes extracted from 
distance data acquired by a stereo vision system or the like and climbing the stairs 
on the basis of the result of stair recognition. 

(1) Robot apparatus 

First, a bipedal walking robot apparatus will be illustrated and explained as 
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an example of the robot apparatus of this type. The robot apparatus is a 
practical-use robot which supports human activities in various living environments 
and other daily lives. It is also an entertainment robot capable of acting 
correspondingly to its internal status (anger, sadness, joy, pleasure, etc.) and 
imitating basic behaviors of the human being. It should be noted that although the 
present invention will be explained as to a bipedal walking robot apparatus as an 
example, a stair recognition unit used in the robot apparatus according to the 
present invention, if installed in a robot apparatus of any other legged locomotion 
type than the bipedal type, can have the legged locomotion robot apparatus climb 
stairs. 

FIG. 4 is a perspective view of a robot apparatus according to the 
embodiment of the present invention, showing the concept of the robot apparatus. 
As shown in FIG. 4, the robot apparatus, generally indicated with a reference 
numeral 201, includes a body unit 202 and a head unit 203, two arm units 204R and 
204L, and two leg units 205R and 205L, each connected in a predetermined 
position to the body unit 202 (note that "R" and "L" indicate "right" and "left", 
respectively; this is also true for the following illustration and description). 

The degrees of freedom of the joints of the robot apparatus 201 are 
schematically illustrated in FIG. 5. The neck joint supporting the head unit 203 
has three degrees of freedom including a neck joint yaw axis 101, neck joint pitch 
axis 102 and neck joint roll axis 103. 
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Also, each of the arm units 204R and 204L forming together each upper limb 
includes a shoulder joint pitch axis 107, shoulder joint roll axis 108, upper arm yaw 
axis 109, elbow joint pitch axis 1 10, lower arm yaw axis 111, wrist joint pitch axis 
1 12, wrist joint roll axis 1 13 and a hand 1 14. The hand 1 14 is actually a 
multipoint, multi-freedom structure including a plurality of fingers. However, 
since the motion of the hand 114 little contributes to or influences the control over 
the attitude and walking of the robot apparatus 201, it is assumed herein for the 
simplicity of the explanation that hand 1 14 has no degree of freedom. Therefore, 
each arm unit has seven degrees of freedom. 

Also, the body unit 202 has three degrees of freedom including a body pitch 
axis 104, body roll axis 105 and body yaw axis 106. 

Also, each of the leg units 205R and 205L forming together each lower limb 
includes a hip joint yaw axis 115, hip joint pitch axis 116, hip joint roll axis 117, 
knee joint pitch axis 118, ankle joint pitch axis 119, ankle joint roll axis 120 and a 
foot base 121. It is defined herein that the intersection of the hip joint pitch axis 
116 and hip joint roll axis 1 17 is the hip joint position of the robot apparatus 201. 
The foot base or sole 121 of the human body is actually a multipoint, 
multi-freedom structure. It is assumed herein for the simplicity of explanation 
that the foot base of the robot apparatus 201 has no degree of freedom. Therefore, 
each leg unit has six degrees of freedom. 

As above, the robot apparatus 201 has a total of 32 degrees of freedom (= 3 
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+ 7*2 + 3 + 6 x 2). However, the robot apparatus 201 destined for entertainment 
has not always the 32 degrees of freedom. It is of course that the entertainment 
robot apparatus can be designed to have an appropriately smaller or larger number 
of degrees of freedom, namely, of joints, depending upon the constraints on the 
design and manufacture, specifications required for the intended robot apparatus, 
etc. 

Each of the above-mentioned degrees of freedom of the robot apparatus 201 
is actually implemented by an actuator. To provide a robot apparatus having a 
shape approximate to the natural shape of the human body without excessive 
apparent bulge and control the attitude of an unstable structure such as a bipedal 
walking robot apparatus, the actuator should preferably be compact and 
lightweight. 

The above-mentioned robot apparatus has provided in the body unit 202, for 
example, a control system to control the motions of the entire robot apparatus itself. 
FIG. 6 schematically illustrates the control system employed in the robot apparatus 
201. As shown in FIG. 6, the control system includes a thought control module 
200 dynamically responsive to a user's input or the like to control the emotion 
judgment and sentiment expression, and a motion control module 300 to control the 
whole-body coordinated motion of the robot apparatus 20 1 , such as the driving of 
actuators 350. 

The thought control module 200 is an independent driving type information 
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processing unit including a central processing unit (CPU) 211 to make calculations 
as to the emotion judgment and sentiment expression, random-access memory 
(RAM) 212, read-only memory (ROM) 213, external storage unit (such as a hard 
disk drive) 214, etc. and which can make a self-contained processing within itself. 

The thought control module 200 determines a current sentiment and will of 
the robot apparatus 20 1 according to an external stimulus or the like such as image 
data supplied from an image input unit 251, sound data supplied from a sound input 
unit 252, etc. That is, the thought control module 200 can recognize the facial 
expression of the user of the robot apparatus 201 on the basis of the input image 
data and make an action corresponding to the user's facial expression by reflecting 
the information on the sentiment and will of the robot apparatus 20 1 . The image 
input unit 25 1 included in the present invention includes a plurality of CCD 
(charge-coupled device) cameras, for example, and can acquire a range image 
based on images captured by the cameras. Also, the sound input unit 252 includes 
a plurality of microphones, for example. 

The thought control module 200 issues a command to the motion control 
module 300 for execution of a sequence of motions or actions based on the 
determined will, that is, motions of the four limbs. 

The motion control module 300 is an independent drive type information 
processor including a CPU 311 to control the whole-body coordinated motion of 
the robot apparatus 201, RAM 312, ROM 313, external storage unit (such as hard 
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disk drive) 314, etc. and which can make a self-contained processing within itself. 
Also, the external storage unit 314 can store a walking pattern, target ZMP 
trajectory and other action plan calculated on offline basis, for example. 

The motion control module 300 has connected thereto, by a bus interface 
(I/F) 310, various units including actuators 350 to implement degrees of freedom of 
the joints located on the whole body of the robot apparatus 201 as shown in FIG. 5, 
distance measuring sensors (not shown) to measure a distance from the robot 
apparatus 201 to an object, attitude sensor 351 to measure the attitude and 
inclination of the body unit 202 of the robot apparatus 201, landing checking 
sensors 352 and 353 to detect when the robot apparatus 201 lifts the right or left 
foot base 121 from a surface or places the foot base on the surface, load sensor 
provided on the foot base 121, power controller 354 to control the power source 
such as a battery or the like, etc. It should be noted here that the attitude sensor 
351 is for example a combination of an acceleration sensor and gyro sensor and 
each of the landing checking sensors 352 and 353 is formed from a proximity 
sensor, microswitch or the like. 

The thought control module 200 and motion control module 300 are built on 
a common platform and connected to each other by a bus interface 210 and the bus 
interface 310. 

The motion control module 300 controls the whole-body coordinated motion 
made by the actuators 350 to implement an action commanded from the thought 
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control module 200. More specifically, the CPU 311 calls a motion pattern 
corresponding to a motion commanded from the thought control module 200 from 
the external storage unit 314 or internally creates a motion pattern. Then, the CPU 
311 sets, according to the specified motion pattern, a foot motion, ZMP (zero 
moment point) trajectory, body motion, upper-limb motion, horizontal position and 
height of the hip, etc. and transfers command values commanding motions 
corresponding to the settings to the actuators 350. 

Also, the CPU 311 can adaptively control the whole-body coordinated 
motion of the robot apparatus 201 by detecting the attitude and inclination of the 
body unit 202 of the robot apparatus 201 on the basis of an output signal from the 
attitude sensor 351, while detecting, based on output signals from the landing 
checking sensors 352 and 353, whether the leg unit 205R or 205L is in swinging 
phase or in supporting phase. Further, the CPU 311 controls the attitude and 
motion of the robot apparatus 20 1 for the ZMP (zero moment position) to always 
be positioned toward the center of ZMP-stable region. 

Also, the motion control module 300 is designed to return to what extent an 
action corresponding to a will determined in the thought control module 200 has 
been implemented, that is, the progress of operation, to the thought control module 
200. Thus, the robot apparatus 201 can judge its internal and surrounding 
conditions on the basis of a control program, and act autonomously. 

(2) Motion controlling method for the robot apparatus 
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In the aforementioned robot apparatus, the head unit 203 can have installed 
therein a stereo vision system to acquire three-dimensional distance information on 
the surrounding environment. Next, there will be explained a series of operations 
made using the three-dimensional distance data acquired by the stereo vision 
system from the surrounding environment to extract planes recognizing stairs on 
the basis of the result of plane segmentation/extraction and controlling the robot 
apparatus to climb the stairs on the basis of the result of stair recognition. 

FIG. 7 is a functional block diagram of a system in which the robot 
apparatus makes a process from the extraction of stereo data down to stair climbing. 
As shown in FIG. 7, the robot apparatus includes a stereo vision system 1 to 
acquire three-dimensional distance data, a plane segmentation/extractor unit 2 
which is supplied with stereo data Dl from the stereo vision system 1 and detects 
intra-environment planes from the stereo data Dl, a stair recognition unit 3 to 
recognize stairs from plane data D2 supplied from the plane segmentation/extractor 
unit 2, and a stair climber 4 to output a motion control command D5 for causing the 
robot apparatus to climb the stairs on the basis of stair data D4 which is the result 
of recognition supplied from the stair recognition unit 3. 

The robot apparatus views the surrounding environment by means of the 
stereo vision system 1 and outputs, as an image, the stereo data Dl which is 
three-dimensional distance information calculated from the parallax between both 
the eyes. More particularly, the robot apparatus compares pixels included in 
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image inputs from two cameras, right and left, equivalent to the human eyes, 
estimates the distance to an object on the basis of the parallax between the cameras, 
and outputs three-dimensional distance information as an image (range image). 
The robot apparatus can recognize a plurality of planes existing in the environment 
by segmenting the range image and extracting the planes by means of the plane 
segmentation/extractor unit 2. Further, the plane recognition unit 3 extracts, from 
these planes, a plane the robot apparatus can climb, recognizes a stair on the basis 
of the plane, and supplies the stair data D4 to the stair climber 4. The latter will 
use the stair data D4 to output the motion control command D5 which implements 
the stair climbing. 

FIG. 8 A is a schematic illustration showing the robot apparatus 201 taking a 
picture of the surrounding environment. On the assumption that the floor-surface 
plane is x-y and the height direction is z, the robot apparatus 201 having an image 
input unit (stereo camera) installed in the head unit 203 thereof can view a 
predetermined field in front thereof as shown in FIG. 8A. 

In the robot apparatus 201, the CPU 21 1 is supplied with a color image and 
parallax image from the image input unit 251 and all sensor data such as joint 
angles and the like from the actuators 350 to execute software for variety of 
operations. 

The software executed in the robot apparatus 20 1 according to this 
embodiment is configured in units of an object to permit various operations for 
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recognition of the position of the robot apparatus itself, moving distance of the 
robot apparatus, obstacle or obstacles lying around the robot apparatus, 
environmental map, etc., and outputting of a train of actions to finally be done by 
the robot apparatus. It should be noted that this embodiment of the present 
invention uses two coordinates including a camera coordinate system of a 
world-standard type whose origin is a position predetermined based on a specific 
object such as a landmark (will also be referred to as "absolute coordinate" 
hereunder) and a robot-center coordinate system whose center (coordinate origin) is 
the robot apparatus itself (will also be referred to as "relative coordinate" 
hereunder) . 

In the stereo vision system 1, joint angles determined from the sensor data 
are utilized to transform the robot-center coordinate system having the robot 
apparatus 201 fixed at the center thereof into the coordinate system of the image 
input unit 25 1 provided in the head unit 203 at times when the image data including 
the color image, parallax image from the stereo camera, etc. have been captured. 
In this case, the stereo vision system 1 in this embodiment derives a homogeneous 
transformation matrix etc. of the camera coordinate system from the robot-center 
coordinate system and outputs a range image formed from homogeneous 
transformation matrix and corresponding three-dimensional distance data. 

The robot apparatus according to this embodiment can recognize stairs 
included in its field of view, and climb the stairs on the basis of the result of 
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recognition (will be referred to as "stair data" hereunder). Therefore, for climbing 
stairs, the robot apparatus has to make judgment as to the dimensions of the stairs 
to see whether the stair dimensions are larger than the foot bases of the robot 
apparatus itself, whether the step height is such that the robot can climb, and the 
like. 

The robot apparatus according to this embodiment has the foot bases whose 
dimensions are as shown in FIG. 8B. In FIG. 8B, on the assumption that the 
forward^noving direction of the robot apparatus 201 is an x-axial direction and a 
direction parallel to the floor surface and perpendicular to the x-axial direction is a 
y-axial direction, the y-directional width of both the feet when the robot apparatus 
201 stands upright is taken as feet base width, the width of a part before the ankle 
(connection between the leg and foot base) and having the dimensions of the foot 
base is taken as "foot _jront_size" (front-part size of the foot) and the width of a part 
behind the ankle and having is taken as "foot__back_size" (back-part size of the 
foot). 

The robot apparatus 201 extracts stairs as shown in FIGS. 9 and 10 in the 
environment around itself. FIGS. 9A and 10A are front views of the stairs, FIGS. 
9B and 10B are side elevations of the stairs, and FIGS. 9C and 10C are perspective 
views of the stairs. 

Note here that a plane or surface on which the human being, robot apparatus 

■ 

or the like steps when climbing stairs (on which the feet or moving legs are placed) 
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is called "tread" and the height from one tread to a next tread (one step height) is 
called "riser". Also it should be noted that in the following, the stairs will be 
counted as one step, second step, ... as the robot ascends the stairs from the level 
near the ground surface. 

In stairs ST1 in FIG. 9, three steps are included, the riser is 4 cm high, the 
first and second treads are 30 cm wide and 10 cm deep, and only the topmost 
(third) tread is 30 cm wide and 21 cm deep. Also in stairs ST2 shown in FIG. 10, 
three steps are included, the riser is 3 cm high, the first and second treads are 33 cm 
wide and 12 cm deep, and only the topmost (third tread) is 33 cm wide and 32 cm 
deep. The robot apparatus recognizes these stairs as will be described later. 

The plane segmentation/extractor unit 2 detects the distance information 
(stereo data Dl) supplied from a distance measuring unit (range finger)such as a 
stereo vision system and extracts a plurality of planes existing in the environment, 
and outputs the plane data D2. The plane segmentation/extraction can be 
implemented by the scan line grouping method which will be described later as 
well as by the conventional plane segmentation/extraction technique using the 
Hough transformation. To extract a plurality of planes such as stairs from 
distance data including noises however, a scan line grouping method which will be 
described later or the like should preferably be adopted since the method will 
assure an accurate extraction of planes. 

FIGS. 1 1 and 12 show examples of the result of stair extraction. The 
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examples shown in FIGS. 1 1 and 12 are results of stair extraction made by 
acquiring three-dimensional distance data from images of the stairs shown in FIGS . 
9 and 10, and segmenting the data to extract planes. More specifically, FIG. 1 1 A 
schematically illustrates the segmentation of the image of the stair in FIG. 9, and 
FIGS. 11B to 11D show the three-dimensional distance data acquired from images 
in FIG. 1 1 A. Also, FIG. 12A schematically illustrates the segmentation of the 
image of the stair in FIG. 10, and FIGS. 12B to 12D show the three-dimensional 
distance data acquired from images in FIG. 12A. As in the examples shown in 
FIGS. 1 1 and 12, all the treads could be extracted as planes. FIG. 1 IB shows an 
example in which the first, second and third treads have been extracted through the 
three-dimensional distance data segmentation/plane extraction process. As will be 
seen in FIG. 12B, a part of the floor surface has also been extracted successfully as 
a plane. 

More specifically, as shown in FIG. 13A when the planes are extracted 
through the segmentation of a range image of the stairs ST2 and extraction, for 
example, regions A to D are extracted as planes indicative of the floor surface, first 
step, second step and third step, respectively, as shown in FIG. 13B. A group of 
points included in the same region included in the regions A to D indicates a group 
of distance data points estimated to form the same plane. 

The stair recognition unit 3 is supplied with the plane data D2 extracted by 
the plane segmentation/extractor unit 2 and recognizes the shape of the stairs, that 
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is, the tread dimensions, step height (riser height), etc. The stair recognition unit 3 
recognizes, as stair data, an edge (front edge) (will be referred to as "EE" 
hereunder) at the near side (near the robot apparatus) of a region (polygon) 
included in the tread recognized by the robot apparatus 201 and an edge (back 
edge) (will be referred to as "BE" hereunder) at the far side (far from the robot 
apparatus) of the tread. Then, the stair climber 4 uses the stair data thus acquired 
to control the stair climbing of the robot apparatus. 

Next, the control of stair climbing of the robot apparatus will be described in 
detail. It should be noted that in the following, firstly the stair recognition by the 
robot apparatus will be described, secondly the plane segmentation/extraction by 
the robot apparatus will be described concerning the recognized stairs, and finally 
the plain recognition by the scan line grouping method will be described as a 
concrete method of plane segmentation/extraction. 

FIG. 14 is a block diagram of the stair recognition unit shown in FIG. 7. 
As shown in FIG. 14, the stair recognition unit 3 includes a stair extraction unit 5 to 
extract stairs from the plane data D2 supplied from the plane 
segmentation/extractor unit 2, and a stair merging unit 6 to recognize more 
accurately the stairs by merging timer-series data of the stair data D3 extracted by 
the stair extraction unit 5, namely, a plurality of stair data D3 extracted at different 
times. The stair data D4 resulted from the merging by the stair merging unit 6 is 
delivered at the stair extraction unit 5. 
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The stair extraction unit 5 extracts stairs from the plane data D2 supplied 
from the plane segmentation/extractor unit 2. The plane data D2 supplied from 
the plane segmentation/extractor unit 2 includes a plurality of information per plane 
as will be shown below. Namely, the stair extraction unit 5 will be supplied with 
plane data per a plurality of planes extracted from images acquired by the stereo 
vision system 1 . 

That is, the plane data D2 includes the following information per plane: 
1-1 Number of supporting points 
1 -2 Central point of a plane 

1-3 Plane parameters (normal vector and distance from the origin) 

1- 4 Edges of polygon, defining a plane 

The robot apparatus selects a plane such as a floor surface or tread on which 
it is placing its foot or feet and generally flat surfaces and calculates the following 
information (will be referred to as "stair parameters" hereunder): 

2- 1 Front edge (FE) and back edge (BE) 
2-2 Step height 

The front edge FE and back edge BE recognized by the robot apparatus 
indicate the edges (lines) of a stair tread. On the assumption that the robot 
apparatus is facing a polygon, an edge, near the robot apparatus (near-side edge), of 
the polygon is taken as the front edge FE, and an edge, far from the robot apparatus 
(far-side edge), of the polygon is taken as the back edge BE. That is, a smallest 
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polygon including all points defining a plane, for example, may be determined, and 
a near edge of the polygon may be taken as the front edge FE while a far edge may 
be taken as the back edge BE. Information on the front and back edges FE and 
BE may be taken as information on these end points. Also, information such as 
stair width W, stair length L, etc. can be acquired from the polygon. Further, a 
stair height (riser) may be taken a difference in height between centers of two 
planes in the supplied plane data D2 or a difference in height between two centers 
of gravity with which the polygon has been determined. It should be noted that 
the riser may be a difference in height between the back edge BE of a former tread 
and the front edge FE of a latter tread. 

Also, in the embodiment, the front and back edges FE and BE, and regions 
adjacent to the right and left of a region (safety zone) between the front and back 
edges FE and BE and where it is estimated that the robot apparatus is highly likely 
to move are recognized as margins. These margins are determined as will be 
described later. By determining the margins, it is possible to widely recognize the 
tread region where it is estimated that the robot apparatus can move. Further, 
information such as the number of data points in a group defining a tread, 
information on a reference point defining one centroidal point, etc. may be taken as 
the stair data D3. 

Planes (stairs) which meet the following requirements are extracted from the 
above stair data: 
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3-1 Front and back edges FE and BE are larger in length than a 

predetermined threshold 

3-2 Stair height is smaller than a predetermined threshold 

It is preferable to extract planes which additionally meet the following 

requirements: 

3-3 Stair width W is larger than a predetermined threshold 
3^4 Stair length L is larger than a predetermined threshold 
FIG. 15 shows a flow of operations made in the stair extraction by the stair 
extraction unit 5. As shown in FIG. 15, the stair climber 4 first judges, based on 
the plane parameters in the supplied plane data, whether the input plane which a 
supporting surface, for example, is horizontal or whether it is a plane on which the 
robot apparatus can walk or move (in step SI). Conditions for the plane to be 
horizontal or a plane on which the robot apparatus can move should be set 
according to the functions the robot apparatus is designed to have. For example, 
on the assumption that the plane vector of an input plane is n(n x , n y , n z ), the plane 
can be judged to be horizontal when |sin_in z | > min^ (where min^ is a threshold for 
judgment of the plane to be horizontal or not). For example, when minth = 80 deg. 
with distance data used, accuracy of plane segmentation/extraction, etc. taken in 
account, a plane having a maximum angle of ±10 deg. in relation to the horizontal 
plane may be judged to be horizontal. Also, if an input plane has an angle of 
about ±30 deg. in relation to the horizontal plane but is a plane on which the robot 
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apparatus can walk or move, planes whose angles are within these ranges (±10 or 
±30 deg) should be extracted. If the stair climber 4 has determined the input plane 
not to be horizontal (No in step SI), it exists this operation with informing the 
failure in plane segmentation/extraction, and repeats the operation with next plane 
data. 

If the stair climber 4 has determined the input plane to be horizontal (Yes in 
step SI), it will recognize the edges (shape) of the plane. The stair climber 4 
determines, in step S2, a polygon including an input plane by the convex hull 
algorithm or smoothing by noise removal as in the well-known technique such as 
the Sklansky's algorithm (J. Sklansky, "Measuring Concavity on a Rectangular 
Mosaic", IEEE Trans. Comput. 21, 1974, pp. 1355-1364), Melhnaiis algorithm 
(Melkman A., "On-Line Construction of the Convex Hull of a Simple Polygon", 
Information Processing Letters 25, 1987, p. 11), for example. Then, the stair 
climber 4 determines front and back border lines of this polygon to be stair 
parameters such as a front edge and back edge in step S3. Further in this 
embodiment, the stair climber 4 determines, based on the front and back edges, a 
width W and length L of a plane defined by a stair tread as the border lines and then 
judges whether the values W and L sire larger than predetermined thresholds, in 
step S4. If the stair climber 4 has determined the tread width and length not to be 
larger than the thresholds (No in step S4), it will determine the plane not to be any 
plane on which the robot apparatus can move and repeat the operations in steps S 1 
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to S4 with next plane data. 

If the stair climber 4 has determined the plane width and length to be larger 
than the thresholds (No in step S4), it will determine the plane to be a plane on 
which the robot apparatus can move, calculate right and left margins in step S5 and 
outputs these information as the stair data D3. 

Next, there will be explained how to form a polygon including an input plane 
as a convex hull algorithm as in step S2. FIG. 16 schematically illustrates a 
convex polygon. FIG. 16A shows a region including all supporting points 
determined to be in one input plane (group of distance data points regarded as 
included in continuous regions in one plane), and FIG. 16B shows a convex 
polygon determined from the graphic shown in FIG. 16 A. The convex polygon 
shown in FIG. 16B can be regarded as using a convex hull to determine a minimum 
convex set including given plane graphics (regions including supporting points). 
It should be noted that a point indicated with G, which will be described in detail 
later, is used to determine the width W of a tread. For example, it is a point 
(reference point) such as a center of gravity of a region including supporting points. 

Typical examples of the algorithm for determining a convex polygon by 
such a convex hull include the aforementioned Melkman's algorithm, Sklansky's 
algorithm, etc. FIG. 17 is a schematic illustration for explanation of the 
Melkman's algorithm. As shown in FIG. 17, three of points, PI, P2 and P3, 
included in a given graphic are extracted, the points PI and P2 are connected by a 
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line to each other, and there are drawn straight lines passing by the points PI and 
P3 and the points P2 and P3, respectively. Thus, the graphic is zoned by these 
lines into five areas AR1 to AR5 including a triangle AR4 formed by the three 
points PI, P2 and P3. Then, a polygon is reformed by judging in which area the 
selected point P4 is included. This operation is repeated to update the convex 
polygon. For example, if the point P4 exists in the area AR1, an area defined by a 
line passed through the points PI, P2, P4 and P3 in this order will be a new convex 
polygon. Also, if the point P4 exists in the areas AR3 and AR4, the convex 
polygon is updated to an area defined by a line passed through the points PI, P2, P3 
and P4 in this order and an area defined by a line passed through the points PI, P4, 
P2 and P3 in this order, respectively. On the other hand, if the point P4 is 
included in the area AR4, that is, inside the convex polygon, the latter will not be 
updated. Also, if the point P4 exists in the area AR2, the convex polygon is 
updated to an area defined by a line passed through the points PI, P2 and P3 in this 
order except for the point P3. In this embodiment, a convex polygon may be 
formed for all the supporting points with an area included in each point being taken 
in consideration. 

FIG. 18 is a schematic illustration for explaining how to determine a polygon 
by the Sklansky's algorithm. A polygon extracted using the Sklansky's algorithm is 
called "Weakly externally visible polygon" which needs less computational 
complexity than the Melkmaris algorithm and can thus be calculated at a higher 
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velocity. 

In case a convex polygon including a given graphic is determined, a half 
straight line is drawn from an arbitrary point x on the boundary of the graphic 131 
to a circle 132 including a graphic 141 as shown in FIG. 18A. At this time, in 
case a half straight line not crossing the graphic 131, of the half straight line drawn 
from the point x to the circle 132, can be drawn, the point should be a point 
included in the boundary of the convex polygon. On the other hand, in case a half 
straight line is drawn from another arbitrary point y on the boundary of a given 
graphic 133 to a circle 134 including the graphic 133 as shown in FIG. 18B, a half 
straight line not crossing the graphic 133 cannot be drawn. In this case, the other 
point y should not be included in the boundary of the convex polygon. When a 
graphic is determined only from the points selected by judging the points one after 
another to see whether each of them forms the convex polygon as above, a graphic 
as shown in FIG. 16A will be provided. 

By determining a convex polygon including the graphic, it is possible to 
determine a convex polygon as shown in FIG. 16B can be obtained. According to 
this embodiment, in case a convex polygon as shown in FIG. 16A is to be 
determined with consideration given to the precision, characteristic, etc. of the 
stereo vision system 1, it will circumscribe the graphic shown in FIG. 16 A as 
shown in FIG. 16B. Taking the precision, characteristic, etc. of the camera in 
consideration, a convex polygon inscribing the graphic shown in FIG. 16A, for 
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example, may of course be determined. Either of these methods may selectively 
be used depending upon the inclination of the plane and environmental conditions. 

Also, in case a polygon including an input plane is determined by the convex 
hull algorithm as in step S2 in FIG. 15, a problem will arise with a non^polygonal 
stair. FIG. 19 explains the problem. FIG. 19A shows an input plane with "step 
0" indicating a stair which is not any convex-polygonal. FIG. 19B shows the 
result of representation of "stepO" in a polygonal form by a convex hull. As will 
be seen from FIG. 19B, there is a large difference of a non-convex portion from the 
desirable result of representation. In such a non-convex polygon, a polygon may 
be determined by smoothing with the close-gaps and fit-Line-segments operations. 
Here will be explained the method of determining a polygon including an input 
plane by the smoothing with the close-gaps and fit-line-segments operations. FIG. 
20 is a schematic illustration of the smoothing. FIG. 20 A shows an input polygon 
which is a region including all supporting points determined to belong to one input 
plane (a group of distance data points in successive regions in the same plane), FIG. 
20B shows a gaps-closed polygon resulted from smoothing the input polygon by 
removing discontinuous gaps from the latter, and FIG. 20C shows a smoothed 
polygon resulted from further smoothing of the polygon obtained as shown in FIG. 
20B with the fit-line-segments operation. FIG. 21 shows an example of the 
program for determining a polygon including an input plane by smoothing with the 
close-gaps and fit-line-segments operations. In this illustration, there are 
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explained the close-gaps operation for removing discontinuous gaps from the 
polygon and fit-line^ egments operation for further smoothing the resultant polygon 
with the fit-line-segments operation. 

First, the close-gaps operation will be explained. Three successive ones of 
apexes defining a polygon are selected. If the central one of the apexes is largely 
distant from a straight line connecting the other two apexes, it is removed. This 
operation is repeatedly done with the remaining apexes of the polygon until there 
remain no more apexes to be processed in this way. 

Next, the fitting4ine-segments operation will be explained. Three 
successive ones of apexes defining a polygon are selected, and an approximate 
straight line fitted to the three apexes, respectively, and distances between the 
approximate straight line and apexes are determined by the least-squares method. 
All the distances from the approximate straight line are arranged in ascending order. 
If the distances are smaller than a threshold, the central one of the apexes is 
removed and the positions of the other two apexes are re-calculated based on the 
approximate straight line. This operation is repeatedly done with the remaining 
apexes until there remain no more apexes to be processed in this way. 

Next, the operation made in step S3 of FIG. 15 will be described in detail. 
First, the stair climber 4 calculates the aforementioned stair parameters from the 
acquired polygon (in FIG. 16B, FIG. 20C). FIG. 22 is a schematic illustration for 
explaining how to calculate the stair parameters. As shown in FIG. 22A, the 
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acquired polygon 140 is taken as an area defined by points 141 to 147. It should 
be noted that the line forming the boundary at the front side, as viewed from the 
robot apparatus 201, of the polygon 140 is taken as the front edge FE while the line 
forming the inner boundary is taken as the back edge BE. 

The width W of the stair tread can be W = dl + d2 (dl is the length of a line 
connecting the center Cfe of the front edge FE and reference point G and d2 is the 
length of a line connecting the center C B e of the back edge BE and reference point 
G). 

Note that the reference point G may be generally the center of a plane as the 
tread. For example, the reference point G may be the central one of all the 
supporting points, center of gravity of the polygon 140 or the center of gravity of a 
safety zone 152 defined by connecting the end points of the front edge FE and back 
edge BE as shown in FIG. 22B. 

With determining the length L and width W of the stair based on the front 
edge FE, back edge BE and reference point G determined as above, the stair 
climber 4 makes the operation in step S4. The stair length L of the stair may be, 
for example, a shorter one of the length of the front edge FE and that of the back 
edge BE or a longer one of the length of the front edge FE including the right and 
left margins which will be explained below and that of the back edge BE including 
the right and left margins. 

Next, there will be explained how to calculate the margins in step S5. FIG. 
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23 is a schematic illustration for explaining a tread finally recognized tread and 
stair parameters. In this embodiment, it is assumed that margins M, and M 2 are 
provided at the left and right ends, respectively, of the safety zone 152 as shown in 
FIGS. 22B and 23 and the region 151 including the left and right margins Mi and 
M 2 is finally recognized as a tread. To determine the margins Mi and M 2 , the stair 
climber 4 first selects points defining a polygon, if any, protruding to outside the 
safety zone 152 defined by the front and back edges FE and BE. For 
determination of the right margin M 2 , for example, the stair climber 4 will select 
points 142, 143 and 146 shown in FIG. 22A. For determining the right margin M 2 
adjacent to the right side of the safety zone 152, the stair climber 4 selects the point 
142 farthest from the safety zone 152 and draws a perpendicular line from the point 
142 down to the front and back edges FE and BE. The stair climber 3 will 
recognize the region 151 defined by the perpendicular line and front and back 
edges FE and BE as a tread. It should be noted that this margin may be 
determined just by drawing a line through the point 142 and across the front edge 
FE or back edge BE. 

Assume here that the length of the left margin Mi in line with the front edge 
FE is /m/and that of the left margin Mi in line with the back edge BE is Ibm as 
shown in FIG. 22B. Similarly, the length of the right margin M 2 in line with the 
front edge FE and that of the right margin M 2 in line with the back edge BE are rfm 
and rbm, respectively. 
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The effect of the recognition of stairs through determining the front and back 
edges FE and BE from a polygon as above will be explained herebelow. FIGS. 
24A and 24B schematically illustrate two types of stairs, respectively. FIG. 24A 
shows stairs whose tread is rectangular as shown in FIG. 9 and 10, and FIG. 24B 
shows a spiral stair. In the spiral stair as shown in FIG. 24B, the back edge BE is 
not parallel with the front edge FE. So, an algorithm by which a rectangular area 
will simply be extracted from extracted planes may not be applied to such a spiral 
stair. Therefore, as in this embodiment, the robot apparatus can be caused to 
climb even such a spiral stair by determining a polygon from extracted planes and 
determining the front and back edges FE and BE on the basis of the polygon. 

Next, the stair merging unit 6 shown in FIG. 14 will be explained in detail. 
The stair merging unit 6 is supplied with stair data (stair parameters) D3 extracted 
by the stair extraction unit 5 and merges the stair data D3 in a time sequence to 
estimate stair information more accurately and widely. For example, a robot 
apparatus having a narrow field of view will not be able to recognize all stairs at a 
time as the case may be. In such a case, old-stairs data on an old frame and 
new-stairs data on a new frame, for example, are searched for a set of stairs 
spatially overlapping each other and data on the stairs overlapping each other are 
merged together to define new virtual stairs. By repeating these operations until 
no more overlapping stairs exist, it is possible to recognize stairs accurately. 

FIG. 25 shows a flow of operations made in stair-data merging in the stair 
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merging unit 6. First, the stair merging unit 6 is supplied with new-stairs data and 
old-stairs data in step SI 1, and merges all of them into one union in step SI 2. The 
stair merging unit 6 searches the union of merged stair data for stair data spatially 
overlapping each other in step S13. In case any set of overlapping stair data is 
found (Yes in step SI 4), the stair merging unit 6 will merge the stair data together 
and register the merged stair data into the stair data union in step S15. The stair 
merging unit 6 repeats the operations in steps S13 and S14 until no further set of 
spatially overlapping stair data exist (No in step SI 4), and outputs a finally updated 
stair data union as the stair data D4. 

FIG. 26 is a schematic illustration for explaining the merging of stair data 
overlapping each other (as in step SI 3). FIG. 26 shows stair data ST1 1 and ST 12 
spatially overlapping each other. The stair merging unit 6 may judge whether the 
stair data spatially overlap each other on the basis of the difference in height 
(distance) between the two stair data at the reference point G in the two stair data 
ST 11 and ST 12 and the area where the treads including the right and left margins 
overlap each other. More specifically, in case the difference in height between the 
centers of gravity Gn and G12 of two stairs is smaller than a threshold (maxdz) and 
the overlapping area is larger than a threshold (minarea), the stair merging unit 6 
can determine the treads indicated by the two stair data to overlap each other. In 
this case, the stair merging unit 6 merges the stair data ST 11 and ST 12 into stair 
data ST13 on a center of gravity G13 as shown in the lower illustration of FIG. 26. 
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For the merging, the stair merging unit 6 has the area of the outer frame 
including the stair data ST11 and ST12 as a step ST13, the area including the safety 
zone except for the right and left margins of the pre^nerged stair data ST 11 and 
ST 12 as a new safety zone 165 after merged, and regions resulted from exclusion 
of the safety zone 165 from the stair data ST 13 as the margins Mi and M 2 . Thus, 
the stair merging unit 6 can determine the front and back edges FE and BE merged 
by the merged safety zone 165. 

More specifically, the stair merging unit 6 determines opposite end points of 
the front edge FE in the merged stair data ST 13 by comparing the right end point of 
the front edge FE of the stair data ST1 1 with that of the front edge FE of the stair 
data ST 12 and also the left end point of the front edge FE of the stair data ST1 1 
with that of the front edge FE of the stair data ST 12, respectively. The stair 
merging unit 6 has the rightmost end point as a right end point 163 of the stair data 
ST 13 and the leftmost end point as a left end point 164 of the stair data ST13. 
Also, the stair merging unit 6 determines the line position of the front edge FE of 
the stair data ST 13 by comparing the front edge FE of the stair data ST1 1 with that 
of the stair data ST12. The stair merging unit 6 takes the line position nearest to 
the robot apparatus (near-side one) as the line position of the front edge FE of the 
stair data ST13. Similarly, the stair merging unit 6 selects the innermost line 
position of the back edge BE of the stair data ST 13 as the back-edge line position, 
and the rightmost and leftmost end points as right and left end points 161 and 162, 
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respectively, of the back edge BE of the stair data ST13. 

Note that the merging technique is not limited to the above. In this 
embodiment, merging is done with the field of view of the robot apparatus taken in 
consideration so that both the rectangular region defined by the front and back 
edges FE and BE and the merged data ST 13 will be largest. However, in case the 
robot apparatus has a sufficiently large field of view or in case the distance data is 
sufficiently precise, the stair merging unit 6 may take a region resulted just from 
addition of two stair data as merged stair data. Also, the stair merging unit 6 can 
determine the reference point G after the merging by making weighted averaging 
correspondingly to the ratio between the numbers of supporting points included in 
the stair data ST1 1 and ST12, respectively. 

Next, the stair climber 4 shown in FIG. 7 will be explained. The stair 
climber 4 is based on the stair data D4 merged and extracted by the stair extraction 
unit 5 and stair merging unit 6 to control the robot apparatus to climb stairs actually. 
The stair-climb control also includes the stair search. 

The stair climbing implemented in this embodiment may include the 
following five operations (state machine): 

4-1 Search mode 

To acquire range image by means of the stereo vision system installed in the 
head unit, the stair climber 4 has the robot apparatus turn the head unit thereof to 
take a look around in order to gather information on the surroundings. 
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4-2 Align mode 

The robot apparatus stands in front of stairs, and moves over a 
predetermined fixed distance. FIG. 27 explains the align mode of operation. In 
FIG. 27 , a region 170 is taken as a first one of the stair treads the robot apparatus 
recognizes. When in the align mode, the robot apparatus moves to a target 
position (will be referred to as "align position" hereunder) align_distance from the 
center of the front edge FE of the tread 170 in a direction perpendicular to the front 
edge FE. In this case, it is assumed that the present position of the robot apparatus 
is a point 171 and the align position to which the robot apparatus is to move is a 
point 172. When the distance between these points 171 and 172 is larger than a 
predetermined threshold max_d and the angle between the direction perpendicular 
to the front edge FE and a direction of the robot apparatus is larger than a 
predetermined threshold max_a, the robot apparatus will start moving toward the 
align position 172. When the distance and direction are both reached, it is to be 
determined that the align mode has been completed. The above thresholds may be 
max_d = 3 (cm) and max_a = 2 deg., for example. 

4-3 Approach mode 

When in the approach mode of operation, the robot apparatus will move to 
just before the stairs. FIG. 28 is a schematic illustration for explaining the 
approach mode. As shown in FIG. 28, having completed the align mode by 
positioning itself in front of the stairs and moving to the align position 172 
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alignj&istance from the starting position, the robot apparatus 201 will position 
itself in front of the center Cfe of the front edge FE of the tread 170 and move to an 
object position (will be referred to as "approach position" hereunder) a 
predetermined distance approach_x distant from there to climb the stairs. 
4-4 Climb mode 

The robot will climb the stairs on the basis of the stair data acquired through 
the aforementioned stair recognition. When the robot has arrived at a tread and 
viewed a next tread, it will continuously ascend or descend. The robot apparatus 
implements the stair climb mode by continuously moving on the stairs until no 
further stair exists. 

4-5 Finish mode 

When the robot apparatus has ascended the stairs in the stair climb mode, it 
will make sure that it stands on the top stair and move to the center of the top stair. 
When the robot apparatus has descended the stairs in the stair climb mode or when 
it has descended the stairs ST1 as shown in FIG. 9 by turning around, for example, 
it will recognize that it has descended to the floor and exit the stair climb mode. 

The stair climb mode will be described in further detail herebelow. FIG. 29 
shows a flow of operations made in the stair climb mode. As shown in FIG. 29, 
the stair climber 4 once put into action will have the robot apparatus search stairs 
(in the search mode), move to a predetermined position opposite to the searched 
stairs (in the align mode) and approach the first one of the stairs (in the approach 

50 



S05P0303WOOO 

mode) (in step S21). When the robot apparatus has succeeded in the sequence of 
search, align and approach (Yes in step S22), it will climb the stairs as will be 
described in detail later (in step S23) and informs the success in stair climbing. If 
the robot apparatus has failed in the above search, align and approach modes of 
operation (No in step S22), the stair climber 4 will exit the series of stair climbing 
operations with informing the failure in stair climbing. In this case, the stair 
climber 4 will have the robot apparatus repeat the operations starting at step S21. 

Note that the sequence of search, align and approach in step S21 is the same 
as a control method used for the robot apparatus to normally arrive at an object or 
destination. More specifically, the following method is available, by way of 
example. FIG. 30 shows a flow of operations made in the search-align-approach 
sequence. 

As shown in FIG. 30, when the search-align-approach sequence is started, 
the robot apparatus gets in a search mode (1) in step S32. When in the search 
mode (1), the stair climber 4 will have the robot apparatus turn the head unit thereof 
to take a look around in order to gather information on the environment as wide as 
possible. As a result, the stair climber 4 judges in step S32 whether the 
surroundings include any stairs the robot apparatus can climb. It should be noted 
here that when the height n z of a plane included in the first one of the extracted 
stairs meets the condition that step_min_z < n z <step_max_z, the stair climber 4 
determines that the stairs are satisfactory for the robot apparatus to climb. When 
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there are stairs the robot apparatus can climb (Yes in step S3 2), the stair climber 4 
will have the robot apparatus align itself to move over a predetermined distance 
(align_distance) to the stairs in order to re-organize the stairs from a near position 
(in step S3 3), and thus re^recognize the stairs it is going to climb (step S34). The 
operation in step S34 is the "search mode (2)". 

The stair climber 4 re-recognizes whether the stairs are satisfactory for the 
robot apparatus to climb (in step S35). When it has also succeeded in the search 
mode (2), it will make sure whether the robot apparatus is opposite to the 
re^ecognized stairs and has moved over the align-distance to the align position, 
namely, whether it has succeeded in the align mode in step S33 (in step S3 6). 
When there are stairs the robot apparatus can climb and the stair climber 4 has had 
the robot apparatus align itself to the stairs (Yes in steps S35 and S36), the stair 
climber 4 will have the robot apparatus get into the approach mode of operation in 
which it will move to the front edge of the first stair in step S37. On the other 
hand, if it has been determined in step S35 that the stairs are not satisfactory for the 
robot apparatus to climb, the stair climber 4 returns to step S31. If the robot 
apparatus has not succeeded in the align mode in step S36, the stair climber 4 will 
repeat the aforementioned operations in step S33 and subsequent steps. 

Next, the stair climb mode of operation in step S22 will be described in 
detail. The stair climb mode consists of a process 1 in which the robot apparatus 
itself can recognize, from a tread on which it is currently standing, a one-step 
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higher or lower tread (will be referred to as "next tread" hereunder), a process 2 in 
which the robot apparatus cannot recognize the next tread from the currently 
standing tread but a two-steps higher or lower tread, and a process 3 in which the 
robot apparatus can recognize a plurality of stairs. FIGS. 31, 33 and 34 show 
flows of operations made in the above stair climbing processes 1 to 3 included in 
the stair climb mode of operation. It should be noted that in the following 
description, it is assumed that the tread (including the floor) on which the robot is 
currently moving is step-O, a next stair is step-1, a stair after next is step-2 and a 
stair m steps higher or lower is step-m. 

First, there will be explained the stair climbing process in which the tread 
step-1 can be viewed and recognized. As shown in FIG. 31, when the stair 
climbing process 1 is started, the stair climber 4 will have the robot apparatus climb 
the stairs (climb operation (1)) (in step S41). In this climb operation (1), the stair 
climber 4 will select a mode of climbing by judging which the stair height n z is, 
positive or negative, since the stair height n z has been recognized in step S32. 
That is, when n z < 0, the stair climber 4 will have the robot apparatus descend the 
stairs. When n z > 0, the stair climber 4 will have the robot apparatus ascend the 
stairs. The value of a control parameter used for the climb operation varies 
depending upon whether the robot apparatus ascends or descends the stairs as will 
be described in detail later. That is, just by selecting a control parameter, the stair 
climber 4 can have the robot apparatus ascend or descend the stairs. 
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Then, the stair climber 4 judges, in step S42, whether the climb operation (1) 
has successfully been done or not. In case the climb operation (1) has 
successfully been done (Yes in step S42), the stair climber 4 will have the robot 
apparatus make a search operation (3). With this search operation (3), the robot 
apparatus moves the head unit having the stereo vision system installed therein to 
acquire distance data on the surroundings and extract a next stair. This search 
operation (3) is nearly similar to the search operation (2) in step S34 in FIG. 30. 

When the search operation (3) has successfully been done as in step S35 in 
FIG. 30 (Yes in step S44), the stair climber 4 repeats the operation in step S41 
again. If no stairs have been found (No in step S44), the stair climber 4 
determines that the robot apparatus has ascended to the top tread or descended to 
the floor, and exits the search operation (3) with making the finish mode of 
operation (in step S45) such as movement to a predetermined position such as the 
tread center. 

Next, the climb operation (1) in step S41 will be described in detail. FIG. 
32 is a schematic illustration of stair treads the robot apparatus has recognized or is 
going to recognize. As shown in FIG. 32, it is assumed that the foot bases 121L 
and 121R of the currently moving robot apparatus are on the tread 181, for example. 
In FIG. 32, the robot apparatus will recognize, as a tread, the safety region defined 
by the front and back edges FE and BE and right and left margins Mi and M 2 
adjacent to the safety region. At this time, the robot apparatus has been able to 
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recognize the tread 182 of a next stair (step-2). It is assumed here that between 
the treads 181 and 182, there exists a gap 184 defined by the riser or the like. 
With the climb operation (1), the stair climber 4 judges whether the robot apparatus 
can move from the tread 181 of the present tread (step-O) to the tread 182 of the 
next stair (step-1). When the tread 181 meets the following conditions, for 
example, the stair climber 4 determines that the robot apparatus can move so: 

5-1 The tread 181 is sufficiently near a front edge FEi of the tread 182 of 

the next stair (step-1) and the angular difference between these treads is 

smaller than a predetermined threshold 
5-2 The tread 182 of the next stair (step-1) has sufficiently large 

dimensions 

5-3 A distance front__x from the front edge FEi to the back edge of the 

foot bases 121Lor 121Ris larger than a control parameter front __x_limit 

in a specified mode of climbing 
5-4 A distance back_x from a back edge BEi to the front end of the foot 

base 121L or 121R is larger than a control parameter back_x_limit in the 

mode of climbing 

When the robot apparatus has been able to view the tread 183 of the next 
stair (step-2), the stair climber 4 can judge, based on a difference (z2 - zl) between 
the height zl of the tread 182 of the next stair (step-1) at the reference point and 
height z2 of the tread 183 of the stair after next (step-2) at the reference point, 
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which the climbing from the tread 182 of the next stair (step-1) to the tread 183 of 
the stair after next (step-2) is, ascending or descending. It should be noted that if 
the tread 183 of the stair after next (step-2) cannot be recognized, it may be 
considered that the current climbing is maintained. 

In case the stair climber 4 has determined based on the above conditions 5-1 
to 5-4 that the robot apparatus can climb the stairs, the robot apparatus can climb so. 
On the tread 181 of the present tread (step-O), the robot apparatus is aligned with 
the back edge BEq of the tread 181. If the gap 184 between the treads 181 and 
182 is large, the robot apparatus will align itself with the front edge FEi of the tread 
182 of the next stair (step-1), then move to the next stair and align itself with the 
back edge BEi of the next stair. Then when in a next climb mode, the robot 
apparatus will align itself with the front end FE 2 of the tread 183 of the stair after 
next (step-2), move to the tread 183 and align itself with the back edge BE 2 . That 
is, the climb mode of operation is such that the robot apparatus aligns itself with the 
front edge FE of the tread of a next stair, climbs the stairs and aligns itself with the 
back edge BE of the tread of the stair to which it has moved, for example. 

Also, when the gap 184 is small, the robot apparatus may align itself with 
only one of the back edge BE 0 of the tread 181 of the present tread (step-O) and 
front edge FEi of the tread 182 of a next stair under the assumption that these back 
and front edges are generally coincident with each other. Namely, the robot 
apparatus may also align itself with the back edge BEq of the tread 181 of the 
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present tread, move to the tread 182 of a next stair and align itself with the back 
edge BE! of the tread 182 or align itself with the front edge FEi of the tread 182 of 
a next stair, move to the tread of the next stair and further align itself with the front 
edge FEi of the tread 183 of the stair after next. In this case, the climb mode of 
operation is such that the robot apparatus moves to a next stair without aligning 
itself with the front edge FE of the next stair and then aligns itself with the back 
edge BE of the tread of the stair to which it has moved. 

The climb operation (1) is applicable when the robot apparatus can view the tread of a 
next stair (step-1) to which it can move during stair climbing. For example, a bipedal 
walking robot apparatus should have installed therein the stereo vision system 1 capable of 
viewing the foot bases of the robot apparatus. 

In the aforementioned stair climbing processes 2 and 3, the robot apparatus 

cannot view and recognize the tread of the next stair (step-1) from on the tread of 

the present tread (step-O) because of the limited angulation of the connection 

between the head and body units of the robot apparatus but can recognize the tread 

of the tread after next (step-2) or a m-steps higher or lower tread (step^ri), which 

» 

are different from the stair climbing process 1 in which the robot apparatus can 
view and recognize the tread of the next stair. The stair climbing processes 2 and 
3 will be explained herebelow. First, in case the robot apparatus can recognize the 
tread of a stair after next (step-2), when the stair climbing process 2 is started, the 
robot apparatus will make the search operation (4) as above (step S51) as shown in 
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FIG. 32. With this search operation (4), the tread of the stair after next (step-2) is 
viewed and recognized by the stereo vision system 1 . The search operation (4) is 
similar to the operation in step S43 except for recognition of the tread of the stair 
after next (step-2). 

Then, the robot apparatus makes a climb operation (2) in step S52. The 
climb operation (2) is similar to the climb operation (1). Also in this operation, it 
can similarly be judged based on the height n z of the tread of the next stair (step-1) 
which the climbing should be, ascending or descending. The tread of the next 
stair (step-1) is a tread viewable from the tread of a stair to which the robot 
apparatus has moved before it had moved to the tread of the present tread (step-O). 

In case the climbing operation has successfully been done (Yes in step S53), 
if the result of search made in step S51 reveals that the two-steps higher or lower 
tread (step-2) has been extracted (Yes in step S54), the stair (step-2) is updated as a 
next stair (step-1 = step-2) in step S56. The stair climber 4 will repeat the 
operations in step S51 and subsequent steps. If the two-steps higher or lower 
tread (step-2) has not been extracted in the search operation (4) in step S51 (No in 
step S55), the robot apparatus will make the finish operation in step S55, and exit 
this series of operations. 

Note that although the stair climbing processes have been explained under 
the assumption that a two-steps higher or lower tread can be viewed, the stair 
climbing processes can be executed in the similar manner even if a stair three steps 
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higher or lower can be viewed. 

A stair climbing operation in which a tread a plurality of steps (m steps) 
higher or lower can be viewed and recognized will be explained herebelow as the 
stair climbing process 3. 

In case the robot apparatus climbs stairs of which the first to n-th ones have 
been viewed and recognized, a search operation (5) is first done as shown in FIG. 
34. The search operation (5) is basically similar to the search operation (4) in step 
S51 except that the treads of the stairs up or down to a stair m steps higher or lower 
(step-in) which can be recognized are to be viewed. 

The climb operation (3) is effected for k steps in step S62. Also in this 
climb operation (3), a mode of climbing, ascending or descending, can be 
determined based on the difference in height between a plurality of treads having so 
far been viewed. Namely, when the height difference Zi ~Zi-j between the i-th and 
(i-l)th treads is negative, the descending mode should be selected. When the 
height difference is 0 or positive, the ascending mode should be selected. 
Information on the tread on which the robot apparatus moves in this climb 
operation (3) is data having been viewed m steps before the robot apparatus is on 
the present tread. 

When the robot apparatus has successfully climbed the stairs (Yes in step 
S63), the stair climber 4 judges in step S64 whether the robot apparatus has been 
able to view treads higher or lower than the present tread, namely, whether m + n > 
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k. When m + n > k (Yes in step S64), the stair climber 4 will update step~(k +1) 
to step -{n + m) as step-1 to step-{m + n -k) (where n = m) in step S66, and repeat 
the operations in step S61 and subsequent steps. On the other hand, if m < 0 (No 
in step S64), there exists no tread to which the robot apparatus moves. Therefore, 
the stair climber 4 exits the series of operations with executing the finish operation 
in step S65. 

As having been described in the foregoing, the stair climber 4 can have the 
robot apparatus ascend or descend the stairs in the same manner just by selecting 
control parameters for use in the stair climbing processes 1 to 3, respectively, in the 
climbing operation. The control parameters used in the stair climbing are 
intended for limiting the position of the foot base of the robot apparatus in relation 
to the present tread. 

FIG. 35A explains the relation between a tread the robot apparatus has 
recognized and the foot base, and FIG. 35B shows an example of the control 
parameters used in the climb operation. 

The control parameters shown in FIG. 35 A are as follows: 
step_min_z Minimum height difference (riser) between present and next 

treads, over which robot apparatus can climb 
step_max_z Maximum height difference (riser) between present and next 

treads, over which robot apparatus can climb 
ad(align_distance) Distance between front edge FE and robot apparatus 
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which is in align position 
ax(approach_x) Distance between front edge FE and robot apparatus 

which is in approach position 
front_x_limit Limit value (minimal x-value) of distance between tread 

front-edge FE and back end of foot base 121 
back__x_limit Limit value (maximal x-value) of distance between tread 

back-edge BE and front end of foot base 121 
back_x_desired Desired value of distance between back edge BE and 

front end of foot base 121 
In ascending the stairs, if the riser is smaller than step_minjz^ it will not be 
regarded that there exists any step. If the riser is larger than step_max__z, it will be 
determined that the robot apparatus cannot ascend the stairs. Similarly, in 
descending the stairs, if the riser is larger than step_mox_z, it will not be regarded 
that there exists any step. If the riser is larger than step_min_z 9 it will be 
determined that the robot apparatus cannot descend the stairs. 

The control parameter align_distance is used in the align operation alone. 
It is used for starting the stair climbing process to ascend/descend the stairs, that is, 
for climbing the first stair. Similarly, the control parameter approach_x is used in 
the approach operation alone. It is used for starting the stair climbing process to 
ascend/descend the stairs. 

The control parameters front_x_limit and back_x_limit define the relation 
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between a tread the robot apparatus has recognized and the foot bases of the robot 
apparatus. When the distance between the tread back- and front-edges BE and FE 
and the end of the foot base, that is, a portion of the tread, which will not be 
stepped on under the foot steps of the robot apparatus that will move to the tread, is 
smaller than the values of these parameters, it will be determined that the robot 
apparatus will not be able to move to the tread or to make any next stair climbing 
even if it could have moved to the tread. It should be noted that when both the 
values of the parameters front __x_limit and back_x_limit are negative, the tread may 
be smaller than the foot bases. That is, it will be determined that the robot 
apparatus can ascend the stairs even if the tread is smaller than the foot bases. 

The parameter back_x_desired is a distance between the back edge BE of the 
present tread and the front end of the foot base when the robot apparatus is to align 
itself with the back edge BE. As will be seen in FIG. 35B, when the robot 
apparatus is going to ascend the stairs, the parameter back_x_desired is normally a 
distance to a point short of the back edge BE, that is, a point 15 mm short of the 
back edge BE in this embodiment. When the robot apparatus is going to descend 
the stairs, the back_x_de sired will be a distance to a point to which the foot base 
protrudes over the back edge BE, namely, a point 5 mm over the back edge BE in 
this embodiment. That is to say, ascending the stairs will need some distance or 
clearance for lifting and moving the foot to a next tread while descending the stairs 
will need no such distance, and a position protruding from the tread will enable the 
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robot apparatus to easily view and recognize a next tread and subsequent treads. 

FIGS. 36 and 37 are traced drawings of the robot apparatus actually climbing 
the stairs on the basis of the control parameters shown in FIG. 35. FIG. 36 shows 
the robot apparatus ascending the stairs. The frames show the sequential motions 
of the robot apparatus ascending the stairs from the floor to the top stair. The 
frame No. 1 shows the robot apparatus in the search operation (1) in step S31. 
The frame No. 2 shows the robot apparatus in the align operation in step S33. The 
frame No. 3 shows the robot apparatus in the search operation (2) in step S32. 
The frame No. 4 shows the robot apparatus in the approach operation in step S37. 
The frame No. 5 shows the robot apparatus in the search operation (4) in step S51. 
The frame No. 6 shows the robot apparatus in the climb operation (2) in step S52. 
The frame No. 7 shows the robot apparatus still in the climb operation (2) in step 
S52 and also aligning itself with the back edge BE of the present tread. The frame 
No. 8 shows the robot apparatus in the search operation (4) in step S51. ... The 
frame No. 17 shows the robot apparatus having not viewed the next tread by 
effecting the search operation (4). The frame No. 18 shows the robot apparatus 
making the finish operation. 

FIG. 37 shows the robot apparatus descending the stairs. As in the stair 
ascending sequence in FIG. 36, the robot apparatus repeats the search operation (as 
in frame Nos. 1, 4, 7, 10, 13 and 16) and climb operation (including the align 
operation as in frame Nos. 5, 6, 8, 9, 11, 12, 14 and 15), makes the finish operation 
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(as in frame No. 18) when no more treads are viewed, and exits the stair climbing. 

Next, there will be illustrated and explained a variant of the embodiment of 
the present invention, having been described in the foregoing. The operations of 
ascending and descending the stairs have been explained in the foregoing. 
However, the algorithm included in the control method according to the 
aforementioned embodiment can be applied to move the robot apparatus on a step 
formed from one stair or a one-step concavity, if any, when the step is smaller than 
a predetermined threshold as well as on the stairs as above. 

First, there will be explained a motion of the robot apparatus to ascend a 
single step. FIG. 38 shows a geometric relation between the single step and the 
foot bases of the robot apparatus. A reference numeral 191 in FIG. 38 indicates a 
step whose height zl from the floor surface (z = 0) is 30. Here will be described a 
movement of the robot apparatus from the lower level of the step 191 which will be 
a next tread (stepl) to an upper level as in FIG. 38. It is assumed that the height 
ZO of the moving plane at the lower level of the step 191 is zO = 0 while the height 
z2 of the moving plane at the upper level of the step 191 is z2 = 0. On the 
assumption that the robot apparatus is currently moving on the moving plane 
having the height zO, since zl - zO = 30 > 0, the movement of the robot apparatus 
from on the step 191 to a next region can be determined to be "ascending" and also, 
since z2 - zO = -30 < 0, such a movement can be determined to be "descending". 
Thus in the climb operation, the aforementioned control parameters should be 
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changed in value according to such determination. It should be noted here that in 
case the control parameters shown in FIG. 35B are adopted, values of both the 
parameters front _xJLimit and back_x_limit for the robot apparatus to ascend the 
step are negative, which means that even if the foot bases 121 are protruding from 
the step 191 as shown in FIG. 38, the robot can be determined to be able to move. 

Next, there will be explained a motion of the robot apparatus to step down 
into a single concavity. FIG. 39 shows a geometric relation between the single 
concavity and the foot bases of the robot apparatus. A reference numeral 192 in 
FIG. 39 indicates a concavity whose depth from the floor level (z = 0) is zl = -30. 
Here will be described a movement of the robot apparatus from the lower level of 
the concavity 192 which will be a next tread (stepl) to an upper level as in FIG. 39. 
It is assumed that the height ZO of the moving plane at the lower level of the 
concavity 192 is zO = 0 while the height z2 of the moving plane at the upper level 
of the concavity 192 is z2 = 0. On the assumption that the robot apparatus is 
currently moving on the moving plane having the height zO, since zl - zO = -30 < ° 
0, the movement of the robot apparatus into the concavity 192 as a next region can 
be determined to be "descending" and also, since z2 - zl = 30 > 0, the movement 
from the concavity 192 to a next region can be determined to be "ascending". 
Thus in the climb operation, the aforementioned control parameters should be 
changed in value according to such determination similarly to the step 191. It 
should be noted here that in case the control parameters shown in FIG. 35B are 
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adopted, values of both the parameters front __x_limit and back_x_limit for the robot 
apparatus to descend into the concavity are positive, which means that even if the 
foot bases 121 are smaller than the concavity 192 as shown in FIG. 39, the robot 
can be determined to be able to move. 

In this variation, a plane which can be determined for the robot apparatus to 
move on, for example, a horizontal plane or the like, is extracted from the extracted 
planes and a stair tread is recognized based on a polygon including such plane. 
Then, the robot apparatus is made to climb the stairs on the basis of information on 
the front and back edges FE and BE of the stair tread and stair information 
including the height from the floor level. In the stair climbing, the tread is 
searched to see that the tread is satisfactory for the robot apparatus to move on, the 
robot apparatus is aligned with the front edge FE of the tread having been able to be 
searched or back edge BE of the present surface, and the control parameters are 
changed in value by judging, based on a height difference between the next and 
present treads, which one of the climbing modes the robot is going to be in, 
ascending or descending, whereby the robot apparatus can be made to climb normal 
stairs whose tread is rectangular as well as a spiral stair or the like and both the 
ascending and descending motions can be selected in the same manner just by 
changing the values of the control parameters. Therefore, the same controlling 
manner can be used to have the robot apparatus move on the stairs as well as a 
single step, single concavity or the like. 
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Also, since recognized stairs are merged in a time sequence in the stair 
recognition, the stair information can be recognized in a wide range even in a robot 
apparatus whose field of view is limited due to restrictions imposed on the location 
of a stereo vision system installed in the robot apparatus because object stairs are 
too large for the robot apparatus to climb. Also, even if a next tread cannot be 
viewed and recognized due to similar restrictions imposed on the location of the 
stereo vision system when the robot apparatus climbs the stairs on the basis of the 
stair information, the robot apparatus can climb the stairs on the basis of stair 
information already recognized by viewing. 

The plane segmentation/extractor unit adopted in this variant can positively 
extract, by the scan line grouping method, a prevailing plane in the field of view as 
well as even a plurality of planes such as stairs. Also, in the line fitting effected 
for extraction of a plane or planes, the plane segmentation/extractor unit can permit 
a plane extraction robust against any measuring noises by fitting the lines 
adaptively to the distribution of distance data points. 

FIG. 40 shows functional blocks of the plane segmentation/extractor unit 
used in this variant. As shown in FIG. 40, the plane detector, generally indicated 
with a reference numeral 100, includes a stereo vision system 1 as a distance data 
acquiring means for acquiring three-dimensional distance data, and a plane 
segmentation/extractor unit 2 to extract planes included in a range image formed 
from the three-dimensional distance data by the scan line grouping method. The 
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plane segmentation/extractor unit 2 includes a line fitting block 2a to select groups 
of distance data points estimated to be in one plane from the distance data points 
forming the range image and extract lines from each of distance data points in a 
group, and a region growing block 2b to grow one or a plurality of planar regions in 
the image from a line group including all the lines extracted by the line fitting block 
2a. The region growing block 2b selects three arbitrary lines estimated to be in 
one plane from the line group and determines a reference plane on the basis of the 
three lines. Then, the region growing block 2b judges whether the lines adjacent 
to the selected three lines are in one plane. When it determines that the adjacent 
lines are in one plane, it will update the reference plane with the lines as region 
growing lines and grow the region of the reference plane. 

The line fitting block 2a extracts a group of distance data points estimated to 
be in one plane in a three-dimensional space from each of the data trains along 
rows or lines of the range image and generates more than one line from the group 
of distance data points correspondingly to the distribution of the distance data 
points group. More specifically, if it is determined that the distribution is not 
uniform, it will be determined that the data point groups are not in one plane and 
the data point groups are segmented, it is judged whether or not each of the 
segmented groups of data points is distributed uniformly. These operations are 
repeatedly done. In case the distribution is determined to be uniform, lines are 
generated from the group of data points. The above operations are done as to all 

68 



S05P0303WO00 

the data trains to generate a group of generated lines D 1 1 , and the line group D 1 1 is 
delivered to the region growing block 2b. 

The region growing block 2b selects, from the line group Dl 1, three lines 
estimated to be in one plane, and determines a plane which will be the seed of the 
reference plane on the basis of the three lines. By sequentially merging the planar 
region which will be the seed region with lines being in one plane in which the seed 
region also lies to grow the region, the range image is segmented into a plurality of 
planes to output a plane group D2. 

Effecting the above operations when plane information is required for 
bypassing an obstacle or for stair climbing, the robot apparatus 20 1 acquires 
information on planes essential for the walking, such as stairs, floor surface, walls, 
etc. 

To acquire such three-dimensional distance data by means of the stereo 
vision system 1, the stair ST2 should have a texture on the surface thereof. That is, 
since a parallax between two cameras is utilized to image a stair, if an object stair 
has no texture thereon, no parallax can be calculated and no accurate distance can 
be measured. Namely, the accuracy of distance data measurement by the stereo 
vision system depends upon a texture on an object stair. It should be noted that 
the "parallax" indicates a difference between images of a point in a space, formed 
on the left and right cameras (eyes), and varies depending upon the distance of the 
point from the cameras. 
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FIG. 41 explains a means for imparting such a texture, provided in the robot 
apparatus. As shown in FIG. 41, stereo cameras 1 1R and 1 1L forming the stereo 
vision system are provided in the head unit of the robot apparatus, and a light 
source 12 to emit infrared light, for example, is provided in the head unit. The 
light source 12 emits light to a stair ST3 having no texture on it or other object such 
as a thing having no texture or an insufficient texture or a wall to impart a random 
pattern PT. It should be noted that the means for imparting such a random pattern 
PT is not limited to any light source to emit infrared light if it can form such a 
random pattern PT to provide a range image but the robot apparatus itself may 
impart a texture on an object. However, the infrared light can impart a pattern an 
pattern which the human eyes cannot recognize but can be viewed by the CCD 
cameras or the like installed in the robot apparatus. 

Next, the plane segmentation/extractor unit 2 included in the plane detector 
100 will be explained. The plane segmentation/extractor unit 2 detects planes 
using the scan line grouping method in a variant of the aforementioned embodiment 
of the present invention shown in FIG. 42. In the scan line grouping method, an 
image 1 1 captured from a focus F is first processed along the row- or 
column-direction as shown in FIG. 42. Based on the fact that in a line-directional 
image train (image row), distance data points in one plane will form a straight line, 
there is generated a line including distance data points estimated to be in one plane. 
Namely, the scan line grouping method is to estimate and detect a plane in a group 
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of a plurality of thus acquired lines on the basis of the line groups determined to be 
in one plane. 

FIG. 43 shows a flow of operations made in the plane detection by the scan 
line grouping method. As shown in FIG. 43, the plane segmentation/extractor unit 
2 is first supplied with a range image in step S71. Then in step S72, the plane 
segmentation/extractor unit 2 determines lines from data points in each line- or 
column-directional image train in the range image, estimated to be in one plane. It 
extracts lines estimated to be in one plane from the line group to determine a plane 
including the lines in step S73. In step S73, a region which is to be the seed of the 
plane is selected. This selection is made under the condition that three lines 
including one of vertically adjacent rows (or horizontally adjacent columns) should 
be in one plane. The plane in which the seed region defined by the selected three 
lines is taken as a reference plane, and a plane is determined by averaging the three 
lines. Also, a region defined by three lines is taken as a reference planar region. 

Then, the plane segmentation/extractor unit 2 compares, in clearance, the 
straight line including the line- or column-directional image trains adjacent to the 
selected seed region and the reference plane to judge whether they are in one plane. 
If they are determined to belong to the same plane, the plane 
segmentation/extractor unit 2 adds the adjacent line to the reference plane area 
(region growing), updates the reference plane as a plane including the added line 
(plane updating), and repeats these operations until no more lines in one plane exist 
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in the data train adjacent to the planar region. Then, it searches seed regions, and 
repeats the plane updating and region growing until no seed region (three lines) 
exist. Finally, the plane segmentation/extractor unit 2 couples ones of the thus 
acquired region groups, which form together the same plane. In this variant, there 
is provided step S74 in which there will be done a plane recalculation to determine 
a plane again by calculating the line groups in the acquired plane except for ones 
more than a predetermined threshold off the plane. The plane thus determined is 
taken as a final one. This will be described in further detail later. 

Note here that the process to detect lines from the three-dimensional distance 
data and take, as one plane, a region collecting the lines per each same plane is the 
plane detection by the conventional scan line grouping method. However, the line 
fitting method in step S72 in this variant is different from the conventional one. 
That is, even when it is intended to generate lines fitting the distance data points as 
much as possible by determining the lines from the distance data points as above, 
problems such as over-segmentation and under-segmentation will arise unless the 
threshold is changed correspondingly to the accuracy of the distance data. On this 
account, a method of changing the threshold adaptively to the accuracy of distance 
data and noises is introduced into the line fitting in this variant. 

The plane detection by the scan line grouping method will be described in 
further detail with reference to FIG. 43. The plane segmentation/extractor unit 2 
includes a line fitting block 2a which is supplied with three-dimensional range 
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image from the stereo vision system 1 and detects lines estimated to be in one plane 
in a three-dimensional space. To avoid the problem such as the 
over-segmentation or under-segmentation, that is, recognition of a plurality of 
planes as a single plane or recognition of a single plane as a plurality of planes, an 
algorithm (adaptive line fitting) is introduced to fit lines adaptively to the 
distribution of data points. The plane segmentation/extractor unit 2 also includes 
a line fitting block 2a. The adaptive line fitting is such that the line fitting block 
2a first extracts a line as a first one with a relatively large threshold and then 
analyzes the distribution of the data point groups forming a line, as a second one, 
determined by the least squares method which will be described in detail later on 
the basis of the extracted first line. That is to say, it is roughly estimated whether 
the data point groups are in one plane, to thereby extract the data point groups, and 
it is analyzed whether the data points are unevenly distributed in the extracted data 
point groups, to thereby re-estimate whether the data point groups are in one plane. 

This variant uses an algorithm in which when the result of the data point 
distribution analysis is such that the data point groups are in a zig-zag shape which 
will be described in detail later, it is determined that the distribution is uneven, and 
the data point groups are segmented, and these operations are repeatedly done to 
extract lines adaptively to noises included in the data point group. 

FIG. 44 shows a flow of operations made in the line fitting block 2a, namely, 
it shows the operations in step S72 in FIG. 43 in detail. First, the line fitting block 
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2a is supplied with distance data. It extracts a group of data points estimated to be 
in one plane in the three-dimensional space in the line-directional image trains (data 
point train), for example, of the supplied distance data. The group of data points 
estimated to be in one plane in the three-dimensional space may be a set of data 
points whose mutual distance in the three-dimensional space is less than a 
predetermined threshold, for example, 6 cm, and is extracted as a data point group 
(P[0, . . ., n-1]) in step S81. Then, it is checked whether the number of samples n 
included in the data point group (P[0, . . ., n-1]) is larger than a minimum necessary 
number of samples min_n (in step S82). When the number of samples n is smaller 
than the minimum necessary number minjn (Yes in step S82), the process is ended 
with outputting an empty set as the result of detection. 

On the other hand, when the number of samples n is larger than the 
minimum necessary number min_n (No in step S82), a line (chord) LI connecting 
one end P[0] of the data point group (P[0, n-1]) and other end P[n-1] is 
generated as a first line. Then, a data point whose distance from the line LI is 
searched as a point of interest brk from the data point group (P[0, . . ., n-1]), and the 
distance dist is calculated (in step S83). If the maximum distance dist is larger 
than a threshold max_d for segmentation of the data point group (Yes in FIG. 84), 
the data point group P[0, . . ., n-1] is segmented by the point of interest (break 
point) brk into two data point groups P[0, brk] and P[brk, n-1] (in step 
S88). 
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On the other hand, if the maximum distance dist is smaller than the threshold 
max_d for segmentation of the data point group (No in step S84), the least squares 
method which will be described in detail later is used to determine an optimum-line 
equation line from the data point group (P[0, . . ., n-1]) (in step S85) and a line L2 
defined by the equation line is generated as a second line. Then, it is examined in 
step S86 whether the data point groups (P[0, . . ., n-1]) are in a zig-zag shape in 
relation to the line L2. When the data point groups (P[0, . . ., n-1]) are not in the 
zig-zag shape (No in step S86) 5 the process is ended with adding the determined 
line equation line to a line-extraction results list (in step S87). 

If it has been determined in step S86 that the line determined in step S85 is 
in the zig-zag shape (Yes in step S86), the line fitting block 2a goes to step S88 as 
in step S84, and will segment the data point group by the point of interest brk 
whose distance dist has been determined in step S83 into two data points P[0, . 
brk] and P[brk, . . ., n- 1] . If the two data point groups are acquired in step S88, 
the line fitting block 2a will recursively process the two data point groups by 
making the operations in step S81 and subsequent steps. The line fitting block 2a 
will repeat the operations until all the data point groups will not be segmented any 
more, that is, until all the data point groups have been processed through step S87, 
whereby a line-extraction results list having all the lines registered there is provided. 
With such operations, the influence of noises can be removed from the data point 
group P[0, n-1] to accurately extract a group of lines. 
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Note that although it has been described above that the line LI connecting 
the end points of the data point group P[0, n-1] is generated in step S83, the 
line LI may be determined by the least-squares method from the data point group 
P[0, . . n-1] correspondingly to the distribution and nature, for example, of the 
data point group P[0, ...,n-l]. Also, although the point of interest brk is taken as 
a point whose distance to the line LI connecting the end points, it may be a point 
whose distance to a line determined by the least-squares method as above is largest. 
However, in case a point whose distance to a line determined by the least-squares 
method is largest is taken as the point of interest brk or in case there is a plurality of 
points whose such distance is larger than the threshold max_d for segmentation of 
the data point group, the data point group P[0, . . n-1] may be segmented by all 
such points or by more than one selected from the points. 

Next, the least-squares line fitting by the least-squares method as in step S85 
will be described. There will be explained a method of determining an equation 
for a line most fit to the data point groups when n data point groups P[0, . . ., n-1] 
are given. A model of the line equation is given by the following equation (1): 

xcosa + ysina + d = 0 •••(!) 

In this case, the sum of differences between a line equation model and data 
points at one point (xi, yi) of n data point groups P[0, . . ., n-1] can be given by the 
following equation (2): 
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E fit = £ (jc. cos a + y f sin a + <i) 2 • • • (2) 



A straight line most fit to the data point groups can be determined by 
minimizing the sum of the differences given by the above equation (2). The terms 
a and d for minimizing the equation (2) can be determined as given by the 
following expression (3) using a mean value and variance-covariance matrix of the 
data point groups P: 



1 - 25 

a = — tan -1 ^— i d = -(jtcosor + ysina) • • • (3) 

2 S 2 S 2 



where 
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s xy = X ( x i - x )(yi ~ y) 



Next, the zig-zag shape discrimination effected in step S86 will be explained. 
In this zig-zag shape discrimination method, when n data point groups P[0, . . . , 
n-1], straight line Line (a, d) and xcosct + ycosct + d = 0 are given, it is 
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discriminated whether the data point groups P[0, .. ., n-1] intersect the straight line 
Line as shown in FIG. 45A or whether the data points are evenly distributed under 
the influence of noises for example, as shown in FIG. 45B. Basically, the data 
point groups P[0, n-1] appearing successively on the straight line Line is 
counted. When a larger number of data point groups P[0, . . . , n- 1] than a specific 
number appear successively, it can be determined the data point groups form a 
zig-zag shape. In case the data point groups intersect the straight line Line as in 
FIG. 45 A, the data point group P[i] have to be segmented for determination of a 
straight line Line more fit to the data point groups P[0, . . ., n-1]. FIG. 46 shows a 
flow of operations made in the zig-zag shape discrimination method. 

First, the line fitting block 2a is supplied with the data point groups P[0, 
n-1] and straight line Line (a, d, a) in step S90. The "a" indicates a standard 
deviation of the point train. Next, the line fitting block 2a judges whether the 
standard deviation a is larger than a threshold th_ a . If the standard deviation cr is 
smaller the threshold th_a (No in step S91), the line fitting block 2a terminates the 
judgment in order avoid the influence caused by the detection of a floating point 
calculation error of the CPU. The discrimination is continuously done only when 
the standard deviation a is larger than the threshold th_ a. Next, the line fitting 
block 2a judges, based on sing (sdist(P[0])), on which side of the straight line there 
is the first data point P[0] of the data point groups P[0, . . ., n-1], places the result of 
judgment in val 0 and sets the count of a counter which counts successive data point 
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groups on the same side as val 0 (which counter will be referred to as "successive 
point counter; the count of this counter will be referred to as "count" hereunder) to 
one (1) in step S92. It should be noted that "sign(x) n is a function to repeat the 
sign (+ or -) of the value of "x", and "sdist(i)" indicates a positive or negative 
distance from the i-th data point in the straight line Line determined by calculation 
of P[i].xcosot + P[i].ycoscc +d. That is, a sign "+" or "-" is placed in val 0 
depending upon the side of the line Line on which the data point P[0] is. 

Next, a counter to count data points (will be referred to as "data point 
counter" hereunder; count of this data point counter be referred to as "count i" 
hereunder) is set to " 1 " (count "i") in step S93. When the count i of the data point 
counter is smaller than the number of data n (Yes in step S94), the line fitting block 
2a judges, based on sing(sdist(P[i])), on which side of the straight line the data 
point P[i] on which a next data (will be referred to as "i-th data" hereunder) is, and 
places the result of judgment in val (in step S95). Then, the line fitting block 2a 
compares val 0 determined in step S92 and val determined in step S95. When val 0 
and val are different from each other (No in step S96), the line fitting block 2a 
places val in val 0 , places " 1 " in the count count of the successive point counter (in 
step S98), increments the count "i" of the data point counter (in step S100) and then 
repeats the operations in step S94 and subsequent steps. 

On the other hand, when val 0 and val are equal to each other (Yes in step 
S96), the line fitting block 2a determines that the data points P[i-1] and P[i] are on 
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the same side of the straight line Line and increments the count count of the data 
point counter by one (1) in step S97. Further, the line fitting block 2a judges 
whether the count count of the successive point counter is larger than the minimum 
data point count min_c for determination of the data point groups to be in the 
zig-zag shape (in step S99). When the counter is larger than the minimum data 
point count min_c (Yes in step S99), the line fitting block 2a determines the data 
point groups to be in the zig-zag shape, and exits the process with outputting a 
signal TRUE. On the other hand, when the count count of the successive point 
counter is smaller than the minimum data point count mm_c (No in step S99), the 
line fitting block 2a goes to step S100 where it will increment the count i of the 
data point counter (in step S100), and repeats the operations in step S94 and 
subsequent steps. 

Then, the line fitting block 2a repeats the operations in step S94 and 
subsequent steps until the count i of the data point counter reaches the data number 
n, and it will exit the process with outputting a signal FALSE when a state of count 
i > n. 

When n data point groups P[0, . . n-1] and straight line Line (a, d): xcosa + 
ycosa + d = 0 are given, the line fitting block 2a can judge whether the group of 
data point group intersects the straight line Line in a zig-zag shape. Thus, the line 
fitting block 2a can judge in step S86 whether the data point groups should be 
segmented. When the line fitting block 2a has determined that the group of data 
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point group intersects the straight line determined by the least-squares method in 
the zig-zag shape, it determines that the data point groups should be segmented, 
and will go to step S88 where it will be able to segment the data point groups by the 
point of interest brk as a break point. It should be noted that the operations made 
in steps S91 and SI 00 may be expressed as in FIG. 47. 

Also, the zig-zag shape discrimination can be effected by hardware as well 
as the CPU. FIG. 48 is a block diagram of a zig-zag shape discriminator. As 
shown in FIG. 48, the zig-zag shape discriminator, generally indicated with a 
reference numeral 20, includes a direction discriminator 21 which is supplied with 
n data point groups P[0, . . ., n-1], judges on which side of the straight line Line 
each of the data point P[i] is and outputs the result of judgment val, a delay unit 22 
for comparison of the data one after each data point P[i] with the result of judgment 
from the direction discriminator 21, a comparator 23 to compare the result of 
direction discrimination val at the data point P[i] and result of direction 
discrimination val 0 at the data point P[i-1], a successive point counter 24 to 
increment the count when the comparator 23 provides a result that val = val 0 , and a 
comparator 25 to compare the count count of the successive pint counter 24 with 
the minimum data point count min__c read from a minimum data point count storage 
unit 26. 

The zig-zag shape discriminator 20 constructed as above functions as will be 
described below. The direction discriminator 21 determines a straight line Line by 
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the least-squares method from the data point groups P[0, . . ., n-1], determines a 
positive or negative distance between each data point P[i] and straight line Line and 
outputs the sign, positive or negative of the distance. When supplied with the 
positive or negative sign of the distance from the data point [i-1] to the straight line 
Line, the delay unit 22 stores the data until it is supplied with the positive or 
negative sign of the data point P[i] one after the data point P[i-1]. 

The comparator 23 compares the sign, positive or negative, of the data point 
P[i] and that of the data point P[i~l]. When the signs are the same, the 
comparator 23 will output a signal for incrementing the count count of the 
successive point counter 24. When the signs are different from each other, the 
comparator 23 will output a signal for placing one (1) in the count count of the 
successive point counter 24. The comparator 25 compares the count count and 
minimum data point count min_c. When the minimum data point count min_c is 
larger than the count count, the comparator 25 will output a signal indicating that 
the data point groups P[0, . . ., n-1] are in a zig-zag shape. 

Next, the region growing block 2b shown in FIG. 40 will be explained. 
The region growing block 2b is supplied with line groups determined by the line 
fitting block 2a, judges, by fitting the point train to a plane (plane fitting) in which 
plane each of the lines is, and separates a region formed from the supplied line 
groups into a plurality of planes (planar regions). For the separation of a region 
into a plurality of planes, the following technique is adopted. 
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First, the region growing block 2b searches the supplied line groups for three 
adjacent lines estimated to be in one plane. A plane determined based on these 
three lines (reference plane) is the seed of planes. A region including the three 
lines is called "seed region". The region growing block 2b judges, by the plane 
fitting, whether the lines adjacent to the seed region is included coplanar with the 
reference plane. When the lines adjacent to the seed region is included in one 
plane, the region growing block 2b will add the lines as region-growing lines to the 
seed region to grow the region, while recalculating the reference-plane equation for 
the region-growing lines as well. With the above operations, all the lines are 
distributed in a region (plane). 

FIG. 49 is a schematic illustration for explaining the region growing. When 
stairs 31 defined by a plurality of planes exist in an image 30 as shown in FIG. 49, 
it is meant that three lines 32a to 32c indicated each with a thick line, for example, 
have been selected. A region defined by these lines 32a to 32c is a seed region. 
First, one plane (reference plane) P is determined based on these three lines 32a to 
32c. Next, lines lying coplanar with the plane P are selected based on a data train 
33 or 34 adjacent, outside the seed region, to the lines 32a to 32c lying at the 
outermost side of the seed region. It is assumed herein that a line 33a is selected. 
Then, a plane P' including the group of these four lines is determined to update the 
reference plane P. Further, when a line 34a is selected, a plane P" including the 
group of these five lines is determined to update the plane P'. With these 

-7 
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operations being repeatedly done, the second tread of the stairs 3 1 is determined as 
a plane 45 shown as enclosed with a dash line. Thus, the region growing process is 
effected until no further lines exist for addition to a selected seed region. When 
there is no further line, the image 30 is searched for three lines which are to form a 
seed region and the region growing process is repeated. With these operations 
being done repeatedly, the operation in step S3 in FIG. 43 is repeated until there are 
no further three lines for a seed region. 

Next, there will be explained the plane fitting technique for estimation of a 
plane equation formed from the group of data points P[0, .. ., n-1], the seed region 
selecting technique for selection of a seed region on the basis of the estimated plane 
equation, region growing technique for making a seed region larger, and the 
post-processing technique for recalculating regions except for those having large 
errors on the basis of the estimated plane equation. 

The point P in a three-dimensional space is given by P = (x i9 y,, z^, and the 
plane equation is expressed with its normal vectors n(n x , n y , n z ) and a non-negative 
constant d as given by the following equation (4): 

xn x + yn y + zn z + d = 0 •••(4) 

Note that a plane passing through a focus of a stereo camera cannot be 
viewed by the stereo camera. That is to say, since the plane does not pass by the 
focus, the term "d" in the above equation (4) may be d * 0. Therefore, the 
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least-squares method can be used to determine the plane as a value which 
minimizes the value as given by the following equation (5): 



^f(n,rf) = £(pfn + </) 2 - -..(5) 



The optimal solution is determined as n = m/||m|| and d = — l/||m|| where |||| is 
a vector value and m is a solution of a linear system easily obtainable as given by 
the following equation (6-1) using the Cramer's rule which is to solve the 
simultaneous equation by a determinant: 



A m = b • • - (6 - 1) 



where 



A = £p / pf, b=£p ; -..(6-2) 



Even if new data points have been added or data points have been deleted, 
the above solution permits to recalculate the plane parameters just by updating the 
values A and b in the equation (6-2). Further, in the line fitting in this variant, two 
moments (primary moment: average; geometrical moment: variance) E(p) and 
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T 

E(pp ) of the group of n data points are already known, and they can be used to 



update the values A and b as in the following expression (7) for use in plane 
updating of the group of n data points: 



Also, once the parameters n and d of a plane are calculated, the root mean 
square (RMS) residual of the plane equation indicating the extent of deviation of 
the group of n data points from the plane equation (will be referred to as "rms" 
hereunder) can be calculated using the following equation (8). Also in this case, 
the two moments of the n data points can be used to calculate the following 
equation (8): 



As will be known from the above equation (8), when each data point is in a 
determined plane, the rms (p u . . ., p n ) of the plane equation will be zero (0). If the 
value is smaller, it is meant that each data point is well fit to the plane. 

Next, there will be described the technique for searching a seed region and 
technique for making the seed region larger while updating a plane. FIG. 50 



A<- A + ttE(pp r ) 



b <- b + nE(p) 



(7) 




(8) 
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shows a flow of operations made in seed^-egion search and region growing 
processes. As shown in FIG. 50, to search a seed region, three lines l u 1 2 and 1 3 to 
which the line- or column-directional data train having been used in the line fitting 
and whose pixel positions in the lines li and 1 2 and lines 1 2 and 1 3 overlap each other 
in a direction perpendicular to the data train are searched in step S 101 . Each of 
the data points has an index indicating a pixel position in an image. Lines in a 
line-directional data train, for example, are compared based on their own indexes to 
see whether they overlap each other in the direction of the data train. When such 
lines have successfully been found out (Yes in step S102), the above equation (7) is 
used to calculate the above equation (6-1). Thus, the plane parameters n and d can 
be determined, and they are used to calculate root mean square residuals (h, 1 2 , 1 3 ) 
of the plane equation as given by the above equation (8) (in step S103). In case 
the root mean square residuals rms (l u 1 2 , 1 3 ) of the plane equation is smaller than a 
predetermined threshold th rms l such as 1 cm, the three lines are selected as a seed 
region in step S104. When the root mean square residuals rms (1 1? 1 2 , 1 3 ) of the 
plane equation is larger than the predetermined threshold th rms l, the region growing 
block 2b returns to step S101 where it will search lines which meet the above 
requirements. Also, the lines selected as the seed region are excluded from the 
line groups list so that they will not be used in any other plane growing process or 
the like. 

The seed region thus selected is made by the scan line grouping method to 
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grow. That is, there are first searched lines which are candidate ones to be added 
to the seed region in step SI 05. It should be noted that this region includes a seed 
region already updated as well. The candidate lines are a line (1 4 ) adjacent to the 
line l l5 for example, included in the seed region. The pixel positions in these lines 
have to overlap each other as above. When such lines have successfully been 
searched (Yes in step S106), the root mean square residual rms (1 4 ) of the plane 
equation is calculated, and it is judged whether the rms is smaller than a 
predetermined threshold th rms 2 in step S107. When the rms (1 4 ) is smaller, the 
plane parameters are updated in step S108 and the operations in step S105 and 
subsequent steps are repeated. The operations are repeated until no further 
candidate lines exist. When there exist no further candidate lines (No in step 
S106), the region growing block 2b returns to step S101 and searches a seed region 
again. When no seed region is included in the line group (No in step S102), the 
region growing block 2b exits the process with outputting the plane parameters 
having so far been acquired. 

In this variant, the above equation (8) is used for searching a seed region and 
judging whether three lines are in one plane and also used to judge, in growing the 
region, whether the lines are in the reference plane or a plane resulted from 
updating of the reference plane. More specifically, only when the root mean 
square residual rms of the plane equation is less than the predetermined threshold 
(th_rms), the lines (group) are estimated to be in one plane and a plane is 
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recalculated as a plane including the lines. By judging, based on the root square 
mean residual rms of the plane equation, whether the lines are in one plane, a plane 
can accurately be extracted even when the plane includes small steps robust against 
noises. The reason will be explained below. 

FIG. 51 schematically illustrates examples equal in distance between an end 
point and straight line to each other but different in root mean square residual rms 
of the plane equation from each other. As disclosed in Non-Patent Publication 4, in 
region growing with a straight line being coplanar with a plane P when the distance 
D between an end point of the straight line (line segment) of interest and the plane 
P is smaller than a predetermined threshold, a straight line La intersecting the plane 
P (as in FIG. 51 A) and straight line Lb parallel with the plane P and a 
predetermined distance off the latter (as in FIG. 5 IB) will be used to update the 
plane P. The root mean square residual rms of the plane equation is determined. 
The root mean square residual rms (La) of the plane equation, determined based on 
the straight line La in FIG. 51 A is smaller than the root mean square residual rms 
(Lb) of the plane equation, determined based on the straight line Lb in FIG. 5 IB. 
That is, as shown in FIG. 51 A, when the straight line La and plane P intersect each 
other, the root mean square residual rms of the plane equation is relatively small 
and influenced by noises in many cases. On the other hand, as shown in FIG. 5 IB, 
the root mean square residual rms of the plane equation is large and it is highly 
probable that the straight line Lb is not coplanar with the plane P but with another 
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plane P\ Therefore, in case it is necessary to accurately determine a plane from an 
environment including a plurality of planes, it will be preferable to calculate a root 
mean square residual rms of a plane equation and determine lines to be in one plane 
when the rms value is smaller than a predetermined threshold as in this variant. It 
should be noted that when the distance between the end point of a line and a plane 
is smaller than a predetermined threshold, the line may be included in the plane as 
in the conventional technique. The former and latter techniques may be used in 
combination depending upon the environment and nature of distance data. 

Also, once the plane parameters n and d are calculated, the root mean square 
residual rms of the plane equation can easily be calculated by the aforementioned 
equation (8) by updating the plane equation on the basis of the values of two 
moments determined during line fitting of a group of data points. 

Also, the aforementioned selection of a seed region can be expressed as 
shown in FIG. 52. In FIG. 52, "overlapflj, 10" is a function for outputting "true" 
when a position between end points in straight line vectors lj and l k included in each 
image row overlaps the straight line vectors in a position of intersecting the line 
vectors. Also, "fitplane(l l5 1 2 , 1 3 )" is a function for determining a solution of Am = b 
by the aforementioned equations (4) to (7), calculating the plane parameters n and d 
and fitting line vectors 1 1? 1 2 and 1 3 to the plane by the terms A and b calculated by 
the equation (8). 

"rmsQu h, h)" is a function for calculating, using the equation (6), the value 
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of root mean square residual rms of the plane equation as to all the three straight 

* 

lines. Also, " remove (1 1} 1 2 , 1 3 )" means removal of straight lines 1 2 and 1 3 selected 
as forming a seed region from lines[i], lines[i+l] l 2 and lines[i+2], respectively, 
which removal will prevent these straight lines from being used again for 
calculation. 

Also, the region growing may be expressed as in FIG. 53. In FIG. 53, the 
terms A and b are a matrix and vector, respectively, included in the aforementioned 
equation (6-1). Also, "add(A, b, 1)" is a function for adding the moment of a 
straight line Line to A and b using the equation (8). "Solve(A, b)" is used to 
determine the value m which meets Am = b and calculate the plane parameters n 
and d on the basis of the equations (4) to (7). "select(open)" is a function for 
selecting one element arbitrarily from "open". Also, "indexd)" is a function for 
returning the index of "V in the image column or row. Also, neighbor(index) " is 
a function for returning an index adjacent to a given index, for example, {index-1, 
index+ 1 } . 

Also, in this variant, the region growing is done in step S73 in FIG. 43 to 
update the plane equation, and then the plane equation is recalculated (which is a 
post-processing) in step S74, as having previously been described. In this 
recalculation, a displacement, from the plane, of the distance data or lines estimated 
to be in a plane defined by the plane equation finally acquired through the 
aforementioned updating, for example, and the plane equation is updated again 
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with exclusion of the distance data or lines more than a predetermined value off the 
plane, whereby the influence of noises can further be reduced. 

Next, the operation in step S74 will be described in further detail. Here will 
be explained a technique for recalculation of the plane equation in two steps. First, 
when data points nearer to a plane adjacent to a plane in which the data points 
currently exist than the latter plane are detected at distance data points (pixels) at 
the boundary of each plane detected in step S73, the data points are included into 
the adjacent plane. Also, in case there can be detected data points not in any of 
plane but through which there passes a plane whose distance to the data points is 
smaller than a relatively large threshold such as 1 .5 cm, the data points in question 
are included into that plane. These operations can be effected by searching data 
points near the boundary of each planar region. After completion of these 
operations, the plane equation is recalculated. 

Next, in case the distance between each data point and the plane exceeds a 
relatively small threshold such as 0.75 cm at a location near the boundary of each 
region of a plane recalculated as above, such data points are discarded. Thereby, 
the planar region will be somewhat smaller but a more accurate plane can be 
determined. The deletion of distance data points and recalculation of a plane are 
repeatedly done, whereby an extremely accurate plane can be determined. 

Next, the result from each of the above operations will be described. FIG. 
54A is a schematic illustration of a floor surface viewed from the robot apparatus 
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being in upright position, and FIG. 54B shows three-dimensional distance data with 
the vertical axis taken as x, horizontal axis as y and the density of each data point 
taken as z, and straight lines detected from a group of data points included in a 
line-directional pixel train and determined in the line fitting process on the basis of 
to lie in one plane. FIG. 54C shows a planar region determined by the region 
growing from the group of straight lines shown in FIG. 54B. As seen in these 
drawings, only one plane (floor surface) exists in the field of view of the robot 
apparatus, namely, all the floor surfaces are detected as the same plane. 

FIG. 55 explains the difference in result of performance between the plane 
segmentation/extraction method according to the variant and conventional plane 
segmentation/extraction method when a step is placed on a flat surface. As shown 
in FIG. 55A, a step ST3 is placed on the floor F. FIG. 55B shows experimental 
conditions. In case the distance between a point of interest and straight line (line 
segment) exceeds max__d, the data point group is segmented. The column 
"Correct extraction (horizontal)" shows the number of times of succeed in plane 
segmentation/extraction by the line fitting in which a total of 10 times of line fitting 
is done for each line-directional data train, the column "Correct extraction 
(vertical)" shows whether the extraction has successfully be done for each 
column-directional data train. Also, the "Nos. 1 to 5" show the conditions for 
plane fitting by the conventional line fitting method in which the aforementioned 
zig-zag shape discrimination is not adopted, and the "No. 6" shows the conditions 
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for the plane fitting adopted in this variant and adopting g the zig-zag shape 
discrimination. 

FIGS. 55C and 55D show the results of the plane segmentation/extraction by 
line fitting. FIG. 55C shows the result of the plane segmentation/extraction done 
according to the variant, and FIG. 55D shows the results of plane 
segmentation/extraction by the conventional line fitting (as comparative example). 
As seen in FIG. 55B, in case the plane segmentation/extraction is done by the 
conventional line fitting, if the threshold parameter max_d is increased in value 
(max_d = 25 or 30) for the purpose of estimation, the rate of detection will be lower. 
If the threshold parameter max_d is decreased in value (max_d = 10 or 15), the 
detection rate will be higher. Thus it will be understood that according to the 
present invention, however, the introduction of the zig-zag shape discrimination 
permits to provide excellent results of detection even if the threshold parameter is 
increased in value (max_d = 30). 

More particularly, as the threshold parameter max__d is set larger, the noise 
influence will be lower but the line fitting will be difficult. On the contrary, when 
the threshold parameter max_d is set smaller, the line fitting will be erroneous more 
frequently under the influence of noises. FIG. 56 A schematically illustrates an 
image of a floor surface, and FIGS. 56B and 56C show lines fitted by the line 
fitting method adopted in this variant and by the conventional line fitting method, 
respectively, on the basis of horizontal and vertical distance data point trains, 
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respectively, included in the three-dimensional distance data acquired by taking a 
picture of the floor surface shown in FIG. 56 A. The left half of each of FIGS. 
56B and 56C shows an example of the line fitted based on the line-directional pixel 
train (distance data train) and right half shows an example of the line fitted based 
on the column-directional pixel train (distance data train). As will be known from 
FIG. 56B, when the threshold parameter max_d is set smaller, the noise influence 
will be increased. Lines cannot well be detected from a far location where the 
noise influence is large. On the other hand, as seen in FIG. 56C, in case the 
zig-zag shape discrimination is added to the conventional line fitting, even if the 

■ 

threshold parameter max_d is set larger, lines can detected even from a far location 
where the influence of noises is large. 

Thus, plane segmentation/extraction can be done by acquiring 
three-dimensional distance data from an image of different stairs as above. As 
shown in FIGS. 1 1 and 12, for example, all treads can be detected as planes in any 
case. It should be noted that a part of the floor can be detected as another plane as 
shown in FIG. 12B. 

According this variant, for the plane segmentation/extraction by the line 
fitting method, a large threshold is initially set to segment a line. Then, the 
zig-zag shape discrimination is effected to segment a straight line having not any 
larger number of data points than the threshold but having a zig-zag shape taking 
the line as a line formed from a plurality of planes. Therefore, the plurality of 
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planes can accurately be detected based on distance information or data including 
noises. 

Since even a small step can be detected with a high accuracy as above, stairs 
or the like can be recognized in an environment in which the robot apparatus can 
move, for example, the robot apparatus, which is of a bipedal walking type, can 
climb the stairs on the basis of such results of detection. 

Further, any irregular floor surface formed from a plurality of planes will not 
erroneously be recognized as a plane on which the robot apparatus can walk and the 
robot apparatus will be able to move more easily. 

In the foregoing, the present invention has been described in detail 
concerning certain preferred embodiment and variant thereof as examples with 
reference to the accompanying drawings. However, it should be understood by 
those ordinarily skilled in the art that the present invention is not limited to the 
embodiments but can be modified in various manners, constructed alternatively or 
embodied in various other forms without departing from the scope and spirit 
thereof as set forth and defined in the appended claims. Any arbitrary one or more 
of the aforementioned plane segmentation/extraction, stair recognition and stair 
climbing may be implemented by a hardware or a computer program is to be 
executed by a CPU. In the operations are to be performed under the control of the 
computer program, the latter may be distributed in a recording medium or via a 
transmission medium such as the Internet or the like. 
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CLAIMS 

1 . A robot apparatus enabled by a locomotive means to move, the apparatus 
comprising: 

a plane segmenting/extracting means for detecting a plane or a plurality of 
planes included in an environment surrounding the apparatus from 
three-dimensional distance data and outputting the detected plane or planes as plane 
information; 

a stair recognizing means for recognizing, based on the plane information, a 
stair having a plane on which the apparatus can move and outputting stair 
information including information on the tread and riser of the stair; and 

a stair-climb controlling means for controlling the stair climbing of the 
apparatus by judging, based on the stair information, whether the apparatus can 
climb the stair and having the apparatus autonomously position itself in relation to 
the stair tread when the stair is determined to be climbable. 

2. The apparatus according to claim 1, further comprising a distance measuring 
means for acquiring the three-dimensional distance data. 

3. The apparatus according to claim 1, further including legs as the locomotion 
means, on which the apparatus can move. 

4. The apparatus according to claim 1, wherein the stair recognizing means 
includes : 

a stair extracting means for detecting, based on the supplied plane 
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information, a stair having a plane on which the apparatus can move and outputting 
stair information not yet merged; and 

a stair merging means for statistically processing a plurality of 
yet-to-be-merged stair information supplied from the stair extracting means and 
which are temporally different from each other and outputting merged stair 
information. 

5. The apparatus according to claim 4, wherein: 

the stair extracting means recognizes the dimensions and spatial positions of 
a tread on the basis of the plane information and outputs the resultant tread 
information as the yeMo-be-merged stair information; and 

the stair merging means merges, when a group of two or more treads 
overlapping each other in a range larger than a predetermined threshold and whose 
difference in height from each other is smaller than a predetermined threshold has 
been detected from temporally continuous tread information, the tread information 
together into a single piece of tread information including tread information on all 
the treads in the tread group. 

6. The apparatus according to claim 1, wherein the stair recognizing means 
recognizes the dimensions and spatial position of a tread on the basis of the plane 
information, and takes them as the tread information. 

7. The apparatus according to claim 6, wherein the tread information includes 
information on a front edge as the outermost edge, and information on a back edge 

98 



S05P0303WO00 

as the innermost edge, of the tread in at least the moving direction. 

8. The apparatus according to claim 7, wherein the tread information includes 
right- and left-margin information indicative of marginal regions adjacent to the 
right and left sides, respectively, of a safety zone defined by the front and back 
edges and in which it is estimated that it is highly probable that the apparatus can 
move. 

9. The apparatus according to claim 7, wherein the tread information includes 
reference-point information indicative of the center of gravity of a region which is 
estimated, based on the plane information, to be a tread. 

10. The apparatus according to claim 9, wherein the tread information includes 
position information on a reference point which is the center of gravity of a safety 
zone indicating defined by the front and back edges, the center of gravity of a tread 
region adjacent to the safety zone and opposite sides of the latter, which is a 
marginal region in which it is estimated that it is highly probable that the apparatus 
can move or a central point determined based on a group of points forming a plane 
which is a tread. 

11. The apparatus according to claim 7, wherein the tread information includes 
three-dimensional coordinate information on a group of points forming together a 
plane which is to be a tread. 

12. The apparatus according to claim 1, wherein the stair recognizing means 
extracts the boundary of a plane on the basis of the plane information to calculate a 
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polygon, and calculate the tread information from the polygon. 

13. The apparatus according to claim 12, wherein the polygon is a convex 
polygonal region circumscribing the plane boundary extracted based on the plane 
information. 

14. The apparatus according to claim 12, wherein the polygon is a convex 
polygonal region inscribing the plane boundary extracted on the basis of the plane 
information. 

15. The apparatus according to claim 12, wherein the polygon is a non-convex 
polygonal region resulted from smoothing of the boundary of the plane extracted on 
the basis of the plane information. 

16. The apparatus according to claim 1, wherein the plane information includes 
more than one selected for each plane from among a normal vector, plane-boundary 
information, center-of-gravity information, dimensions information and flatness. 

17. The apparatus according to claim 3, wherein the stair-climb controlling 
means controls the apparatus to climb the stair after the apparatus has moved to a 
predetermined position opposite to the back edge of the tread on which the 
apparatus is moving. 

18. The apparatus according to claim 17, wherein the stair-climb controlling 
means controls, when the back edge of the tread on which the apparatus is moving 
cannot be recognized, the apparatus to climb the stair after having moved to a 
predetermined position opposite to the front edge of a next tread to which the 
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apparatus is to move. 

19. The apparatus according to claim 3, wherein the stair-climb controlling 
means controls the apparatus to climb the stair by having the apparatus make a 
series of operations from detection of a next tread to which the apparatus is to move 
up to movement to a predetermined position opposite to the next tread. 

20. The apparatus according to claim 19, wherein the stair-climb controlling 
means searches, when a next tread or a tread after next cannot be detected from a 
position where the apparatus currently stands, the next tread on the basis of the 
previously acquired stair information. 

21. The apparatus according to claim 3, wherein the stair-climb controlling 
means controls the apparatus to move a next tread by having the apparatus move to 
a predetermined position opposite to the back edge of the present tread on which 
the apparatus is moving, then detect the next tread and move to a predetermined 
position opposite to the front edge of the next tread. 

22. The apparatus according to claim 3, wherein the stair-climb controlling 
means controls the apparatus to climb stairs on the basis of parameters specifying a 
position of the locomotion means in relation to a tread. 

23. The apparatus according to claim 22, the parameters are determined on the 
basis of a height of foot lifting or lowering of the leg. 

24. The apparatus according to claim 22, further comprising a parameter 
selecting means for selecting parameter values for an intended stair-ascending or 
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-descending motion. 

25. The apparatus according to claim 1, further comprising: 

a line fitting means for fitting lines for each group of distance data points 
estimated to be in one plane in a three-dimensional space; and 

a planar region growing means for extracting a plurality of lines estimated to 
be in one plane from the group of lines fitted by the line fitting means and 
calculating a plane from the extracted plurality of lines, 

the line fitting means can fit lines adaptively to the distribution of the 
distance data points. 

26. The apparatus according to claim 25, wherein the line fitting means extracts 
a group of distance data points estimated to be in one plane on the basis of the 
distance between the distance data points, and re-estimates, based on the 
distribution of the distance data points in the distance data point group, whether the 
distance data point group exists in one plane. 

27. The apparatus according to claim 25, wherein the line fitting means extracts 
lines from a group of distance data points estimated to be in the one plane, takes, as 
a point of interest, a distance data point whose distance to the lines is largest in the 
group of distance data points, judges, when the distance is smaller than a 
predetermined threshold, whether the distance data points in the distance data point 
group are unevenly distributed, and segments the distance data point group by the 
point of interest when the distribution is uneven. 
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28. The apparatus according to claim 25, wherein the line fitting means extracts 
a first line from the group of distance data points estimated to lie in the one plane, 
takes a distance data point in the group, whose distance from the first line is longest, 
as a point of interest, extracts a second line from the distance data point group when 
the distance is smaller than a predetermined threshold, judges whether a larger 
number of distance data points than a predetermined number exist continuously at 
one side of the second line, and segments the distance data point group by the point 
of interest when the larger number of distance data points than the predetermined 
number exist continuously. 

29. The apparatus according to claim 25, wherein the planar region grouping 
means selects more than one line estimated to be in one plane and calculates a 
reference plane, searches lines estimated to be in the plane in which the reference 
plane lies as grouping lines from the group of lines, updates the reference plane 
with the grouping lines and repeats the grouping of the region of the reference 
plane, and outputs the updated plane as an updated one. 

30. The apparatus according to claim 29, further comprising a plane 
recalculating means for recalculating a plane from the group of distance data points 
except for ones whose distance from the updated plane is larger than a 
predetermined threshold, if any, existing in the distance data point group in the 
updated plane. 

31. The apparatus according to claim 25, wherein the planar region grouping 
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means estimates, based on a difference between a lines -determined plane and 
reference plane, whether the lines lie coplanar with the reference plane. 

32. A method of controlling the motion of a robot apparatus enabled by a 
locomotive means to move, the method comprising the steps of: 

detecting a plane or a plurality of planes included in an environment 
surrounding the apparatus from three-dimensional distance data and outputting the 
detected plane or planes as plane information; 

recognizing, based on the plane information, a stair having a plane on which 
the apparatus can move and outputting stair information including information on 
the tread and riser of the stair; and 

controlling the stair climbing of the apparatus by judging, based on the stair 
information, whether the apparatus can climb the stair and having the apparatus 
autonomously position itself in relation to the stair tread when the stair is 
determined to be climbable. 

33. The method according to claim 32, further comprising the step of acquiring 
the three-dimensional distance data. 

34. The method according to claim 32, wherein the locomotion means is legs of 
the robot apparatus. 

35. The method according to claim 32, wherein the stair recognizing step 
includes: 

the stair extracting step of detecting, based on the supplied plane information, 

104 



S05P0303WO00 

a stair having a plane on which the apparatus can move and outputting stair 
information not yet merged; and 

the stair merging step of statistically processing a plurality of 
yet-to-be-merged stair information supplied from the stair extracting step and 
which are temporally different from each other and outputting merged stair 

♦ 

information. 

36. The method according to claim 35 , wherein: 

in the stair extracting step, the dimensions and spatial positions of a tread are 
recognized on the basis of the plane information and the resultant tread information 
is outputted as the yet-to-be-merged stair information; and 

in the stair merging step, when a group of two or more treads overlapping 
each other in a range larger than a predetermined threshold and whose difference in 
height from each other is smaller than a predetermined threshold has been detected 
from temporally continuous tread information, the tread information is merged 
together into a single piece of tread information including tread information on all 
the treads in the tread group. 

37. The method according to claim 32, wherein in the stair recognizing step, the 
dimensions and spatial position of a tread are recognized on the basis of the plane 
information, and takes them as the tread information. 

38. The method according to claim 37, wherein the tread information includes 
information on a front edge as the outermost edge, and information on a back edge 
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as the innermost edge, of the tread in at least the moving direction. 

39. The method according to claim 32, wherein in the stair recognizing step, the 
boundary of a plane is extracted on the basis of the plane information to calculate a 
polygon and the tread information is calculated from the polygon. 

40. The method according to claim 34, wherein in the stair-climb controlling 
step, the apparatus is controlled to climb the stair after having moved to a 
predetermined position opposite to the back edge of the tread on which the 
apparatus is moving. 

4 1 . The method according to claim 40, wherein the stair-climb controlling step, 
when the back edge of the tread on which the apparatus is moving cannot be 
recognized, the apparatus is controlled to climb the stair after having moved to a 
predetermined position opposite to the front edge of a next tread to which the 
apparatus is to move. 

42. The method according to claim 34, wherein in the stair-climb controlling 
step, the apparatus is controlled to climb the stair by making a series of operations 
from detection of a next tread to which the apparatus is to move up to movement to 
a predetermined position opposite to the next tread. 

43. The method according to claim 42, wherein in the stair-climb controlling 
step, when a next tread or a tread after next cannot be detected from a position 
where the apparatus currently stands, the next tread is searched on the basis of the 
previously acquired stair information. 
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44. The method according to claim 34, wherein in the stair-climb controlling 
step, the apparatus is controlled to move a next tread by having the apparatus move 
to a predetermined position opposite to the back edge of the present tread on which 
the apparatus is moving, then detect the next tread and move to a predetermined 
position opposite to the front edge of the next tread. 

45. The method according to claim 34, wherein in the stair-climb controlling 
step, the apparatus is controlled to climb stairs on the basis of parameters 
specifying a position of the locomotion means in relation to a tread. 

46. The method according to claim 45, further comprising a parameter selecting 
step of selecting parameter values for an intended stair-ascending or -descending 
motion. 

47. A locomotion mechanism movable by locomotion means, the mechanism 
comprising: 

a plane segmenting/extracting means for detecting a plane or a plurality of 
planes included in an environment surrounding the apparatus from 
three-dimensional distance data and outputting the detected plane or planes as plane 
information; 

a stair recognizing means for recognizing, based on the plane information, a 
stair having a plane on which the apparatus can move and outputting stair 
information including information on the tread and riser of the stair; and 

a stair-climb controlling means for controlling the stair climbing of the 
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apparatus by judging, based on the stair information, whether the apparatus can 
climb the stair and having the apparatus autonomously position itself in relation to 
the stair tread when the stair is determined to be climbable. 
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ABSTRACT 

In a robot apparatus, a stereo vision system 1 views the environment around 
the robot apparatus and outputs stereo data Dl as a range image which is 
three-dimensional distance information calculated based on the parallax between 
both the eyes, a plane extraction segmentation/extractor unit 2 detects planes from 
the range image to recognize a plurality of planes existing in the environment 
(plane data D2), a stair recognition unit 3 extracts a plane the robot apparatus can 
climb from the plane data D2, recognizes a stair from the plane and outputs stair 
data D4, and a stair climber 4 outputs a climb control command D5 to implement 
the stair climbing on the basis of the stair data D4. Namely, a locomotion body 
such as the robot apparatus can autonomously climb stairs by acquiring information 
on the stairs. 
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(57) Abstract: A robot device observes the external world by a stereo vision system (1) and outputs stereo data (Dl) as a distance 
image, the stereo data (Dl) being three-dimensional distance information calculated by a parallax of both eyes. A plane detector (2) 
detects planes from the distance image to recognize the planes present in the environmenL From plane data (D2), a stair recognizer 
|/} (3) extracts a plane which the robot can climb up and down, recognizes a stair from that plane, and outputs stair data (D4). A stair 
climb-up/-down controller (4) outputs a behavior control command (D5) for realizing the movement for climbing up and down the 
stair by using the stair data (D4). This enables a mobile body itself to obtain information on a stair to autonomously perform the 
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^^h202(D^^ffi#^ttii- ; 5^^^, #gftfiflia-fe>if352, 353<Z)m^{f 



4- 
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k % CPU311te, ZMP^g^^tcZMP^^J^O^^^^^i^tC P/KVH£ 
(2) P^y ^«Oft^{Sia^jfe 

(Stereo Vision System) li:. ^Tl^tt'v ? 3yv'^TAl^fj^f^T-^Dl^A^ 

£*U rO^V^-^-^Dld^^^cD^S^mi-^^B^ (Plane 
Segmentation/Extractor) 2b, spB^tB^^i^tU^J^^^®^— ?T>2frhV&Wt 
%mWrt-Z>Vg$kMW8& (Stair Recognition) 3i, |^^mi^gP2[Cj;l5^li^tLfcmfi 

&m*)irZ>mg>g-&Mffl%£ (Stair Climber) Ab^mtto 

f^o-cgc m£ tt^> s^TcSgint^-efcs^^x — ^di ^®^tb-cai^ -T5 0 
(sgii®^)i-6o ^^gggt®^^^®^tb^2^j;o-c5Fffi^m-t-2)^^r% m 
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m\^x^ybmm2oit^'b\^m^titc.^yb^^mm^mu^^yb203 
\cm^titzm^Atimm25i(Dmm^^i-^o scorns, ^mm^m^ 
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U±VtzW.<Dm%:<Dyj3\i>\<Dm&feet base widths j&<^#£^fcTj£#(JM£.£ 
!&<D&S&&^gfoot_back_sizebirZ ) i>(Db1rZ> e , 

Ei9{^-rpg^sTitt, &ms3m<Dpgmx&<o. mv-ttf 4 Cm , i, 2©®^® 

^±#^{ii|iS30cm, #^fT#10cnu ft±l£-e&v53l£ S <DB§®(£>^ i|>l30cm, ^ 

tf#2icm^orv^o mio^-ri^sT2^ &Wz&v>V£®re&o . ffi. 

«3_htf 3cm. 1, 24£®<Z>iTO<^#£teitil33cm, HfT#12cnu *±1£-??&33|£ 
B<D|g®cD^ i|>S33cm, ^T#32cmtJfeoTV^ 0 ti^h^g^-^bcOPt^ 

frb3frKm£^-*&&nvx¥m&mvtcfflxibz> 0 -r^h. tuiiAte, ®9 
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@ s 3©g^ffi^¥ffi^m^-CV^f(|^-r o £fc, [§]12Bte, ^ffi^-^fdl 

W CO^# (Front Edge) (gilT, ^n^ho^-^FE^V^o )£, g§®<D Jill] (nztfVhg 
fi^Eg»ravMfJ)0^#(Back Edge) (UT. /^>^x r ^BEiV ^. ) 

Mizxmw-fz>o 

l£&iiJ£§(Stair Extraction) 5 1. r(D|5g^m^5^^{±Sbfc^^— *D3<Dm%k 

^\ziEm\^&%mmi.tt&m%ft?mm%t&m (stair Merging) 6t%#u m& 
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^D2te. ^FSfetc 

1—1 : 3 P®£r^^"^§, | ^0|j£(nurnber of supporting point) 
1-2 : ^^(D cp'btteZM 

1-3 : (2fei£-<^lvK jMjO^iEltt) 

1- 4 : w-mzmfc-tz^nm^mw 

2- 1 :^n>-h3i>yv?FE N ^^y^^i/BE 
2-2:^<Di^$ 

d^sggi^^ti.rv^{|ij(D^#(Mij^#)^/^^^3i^v?BE^-r§o ntLte, m 

ilSW (width), pgl£<£>*£ (length) t£k*(D^m%:nz>Z.bfrX^Z> 0 £fc % ^<Di^$( 
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mm mm^?*-?) (D^h*mm7*-?D3tirz>~b&x%z> 0 

3-2 : m%L<Dm£ (height) tfffi S^itt^T 

3-3 : m&<VimW (width) ^0f^(D^{t^_h 
3-4 : Pgi©I^L (length) #9? J£<£> BHit^± 

(D?B^WHm(n , n , n)£LfdS§-£\ | sin n | >min -^fctUifTK^T'fcS 

x y z — 1 z th 

th 

, mm-t^mmr-?, ¥®&m^ms£#EbTmin =so° ft^u-c, * 

th 
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fc#XD%m%ft% Z.Z.X*fe, #J;tf;£SklanskyCQT/l^yXA(j. Sklansky, "Measuring 
concavity on a rectangular mosaic", IEEE Trans Comput.21,1974,pp.l355-1364) 
Melkman<7)T/V^yX-i> (Melkman A., "On-line Construction of the Convex Hull of 
a Simple Polygon" Information Processing Letters 25,1987 ,p.ll)^£b<D&'^l J ^ > ^ AY 

j»d=»e>. *mM<D]&mz.io^x}z, m^m^m-r^-m^isif^mw (width) 

g£L (length) ^n^o{ii:^^o^{iij:(9^:#v^^^if5*^J^ri-5 

^S^S&tm^ElT fecDMmSl ±.X*foZ>m& fr^yyS4 :No) , W5Tfg& 
l^®-e^^JSffL-&^(D-v— ^^(Left Margin, Right Margin) Srff^L-C^^S 

5), ^bOtf^^X-^D3^L-TtB^-r^o 

& (convex hull) &mm\^tc&<DbTZ>Zb&X%Z> 0 J&:fe5, GT^-T^«, #5&-f 

, mm<Dmw%&£>z>mz&m'tz> i b<Dx\ mx.it^—T^^^h^^ti 

5ii©I-t^^ii (#M) Sr^-t". 

^(DXo^dh^mmvx^nm^^r^v^tDmtvx, ±mjt£$K 

, MelkmanOT^^yXA, SklanskyCDT/^yXA^if^fc^ m 17}*, Melkman<D 



*■ 
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Tyv^yXA^SiWi- 5fcfe©«=a:ia-c*fc«. |gjl7^-rj;5^. ^x.b*vfc»U: 
-a*^^3^Pl, P2, P3ttttlU £P1, P2Sr3g^j»^%5lt, £P1, P3s , P 
2, P3iSrii5iiDt6*5l<o r*U££!9 x 3^P1, P2, P3j&>bfcSH*t7#AR4<Sr£tf5 

VK 0 0H*.tJP435S % ^^tARlH:#^Ei-5#^, PI, P2, P4, P3<DmFPX»1fe Astzfflt 
^m^tltzm^^^tz&^^t^ 0 SS^AR3, AR4JC^P4dS# 

^fVetbPl, P2, P3, P40HR^-e^fc^{cH^7 v c|S^ ) 

pi, P4, P2, P3<Dmfrx&Ajtmfttem*titc.m&tvxa&ftM&^irz>o 

*HH\ femAR2\Z&P4ifitbZ>1fr&Y£s ^P3«^#Pl, P2, P3<D)mFfX& 

Ht*)5, SklanskyCOT^yX^iCiOttttS^tb^^^^fi, Weakly Externally 
Visible Polygoni:P^}^5t)^-e^^ N J^<DSklansky<DT/WitLTfr#lL^6!>ft 

TcmMl3l(Dm^±<oiiM<D^7di^mMl3l^^m32izMVX^WM : k3\< 

<it^-c#^*a-, z<D&te&&fi&(DnR&mf&'fz>Mx*$>z>b-tz>o — mi 
8bk^-tx^ ^ttfrtcm&i33(DmR±i£M<Di&<v&yfrbmj&i33^tt 

z.(omj&k&$irz>&&fi M&$nbz>zbx\ mi6B<D€h^n fc&nz^htfxz 
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•tm^mx&ox, mi9Ate, A^ti^rnxmstepoi-t^^^mw^ 

^^-(D^m^mir^tm^ntc^—^^^^h^xm-^mx^ 
^xmMLtzmmz^ti%b&tbzmM7*-?fiM) &^*MW&xibz>xt)£> 

(input polygon) Hl20Bte. A^¥®&^f ^^^k^^^C^-r^ 

Zf SrBfe^b (close gaps) ¥?IHbUt#*i ^ 0¥*5>:tT&*#£ ff2 : gaps closed 
polygon) Sr^U H20C{*. |^20BT?#b^7 v c#^^^*j-U-C7-f>'^y7 i ^>'^^ 
<£<9(fit line segments) Wt^ttikXJt&ftl&i&ttik&'fa smoothed polygon) -C 

&^%&fim&Mfrz&m<D^vy7j±m&^irmx&v, 

3r^y7%ffe£-rZ)Close gaps^S, RXJP^htltc^^\^MVXy^y^y^> 
?\Z.£.Vg.\Z.^ffiik-tZ>Fit line segments^SI^b-CV ^ c 
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i6B, m2oc)fab±&vtc.m&s<7*-*%nm-rz>o 12122^, m^<^—^ 



i40#\ .^141— &i47\z.xy)mizti&W&x&Z)k<DbirZ> 0 --T\ u#Vb^fi2 

FE 

BE 

it>S W = d 1 + d2b1~Z>Z. b & X% & 0 

^^^t?BE(D^^^^Ay^c|ll22B^^i-^^^152<Dm^iUfcD^r^^-T? 

E^^^iy^^^BEO^^O^^^^^V^irLfc?), ^T^i-&^co-^— 
^^&fc!7u;/hai-,^FE,£.tJ\£;&tf)-^~- i^&^&fc^^y^BEC^ftV^ 

®(3*3V^Tfi, HI22B, HI23{^i-j:5{C > ^^^152<^&^rra^-7— vVM 

12 12 

1 2 
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2 

, £142, 143, 146XhZ> 0 %±M&l52<Dfcimz®mistc^~^M =Sr;fc«>5 

gl22B{c^-rj:5^, yr2^h^yi>F-Etm— fiJfUc&ttSfc-^— v ? vm i 

<£>ft££lmf£U /^5/^^^BEtlH— iiJIUC&ttS;^— v^M^Tg^&lbmi: 
-T5 0 ^n^h^^FE, s<yy-^i?BEt^fo^ftm—WMl~&rtZ>£i^ 

2 

%~b(D%s$k\z.'o\^xmmi,x#>< 0 m2AAm^M2AB\^ 2wm.<DV%w&^-$' j & 

I^T-feS/^ tU24B{4, (spiral) tk<D&g&X&% 0 Z.<Dm24B<D£?t£m%iP£ 
&<DWrh, 7v>b^y¥FE\z.ttVXs<y?^^BEte¥4fXte\f\ Lfc^oT. M 

^n^h^^FE^t^/^^^BE^^fe^wt^J;^, £<Z>J;5fcJ*ifi!RS® 

xh^x^u^ybmm^^Wiip-r^t^mt^o 
^D3^m^m^m^ir^tx\x^iEm^^MWj:m^(Dmm^mM-r^h<Dx 
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^fcOP^S-r— ^ (New Stairs) ^V^ig^-^ (Old Stairs) fc-SrA^fcU* 
tWSII) , fcrU^Rfg*^— #RtffiVl88!7*— *©^T&lo<&£^(ujnion) 

•^^«(^^S14:Yes) , -ttuexOWaSraS^Ur^ttHiaT*— 
:i--5(^T^^S15) 0 ^rUT. ^X5/^°S13, 14<E>*Q.3ffi&^ltB8jK3— 7^ 

&m.w~rz>tiisb(Dm3m-e&z>o 026^ siatttt- ^/vcv^ris?* 

— *ST11, ST12^i~ 0 ^MW^— ^— ^y^bTV^d^d^^JWftJltt, #J 
i«2oOPIlr- ^STll, STl2<Dmmj&G\Z&l?Z>m&<D^(&eM) &£<D 

11 12 

o©pg®7*— ^as^-Tfigffi fit*-— ^y^VCV ^tmWiirZ^kft-eZZo ^ 



\n-im26(DTmi^ir^0^,^^y s ~ *ST11, ST12^-^U-CS^G £> 



13 



7*— ^ST13tt"S 0 

~~T\ itt-a-KlRlbTtt, PSSv*— ^STll, ST12^"tf^#0«^^y^S 
T13£U ^MOfg^T^— ^STll&U*|SgIS:7*— *ST12tf>;fe£<£>-7— >V&P£ 
V ^fe^^*^tf««^iK-a-«©*r^^^l^l65tb. Pg©X-^ST13^ 

1 2 

7*— ^STll<^7n^h^y5^FEfcB©v*— ^STl2<Z>7n>-h^5/i?FE©&^©jS 
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4-1 : If— ^ (Search) Ibfls 
4-2 : 77^ (Align) Ibfls 
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W}m^TVtcbmm-fZ>h<DbirZ> 0 Zfot><Dmm±rC\Z, #l;tfc£max_d = 3(cm) 

x max_a=2° £tZ>Zb&-C%2> 0 
4-3 : TZ?xi—=5- (Approach) ®jf£ 

FE 

approach_x)i^S ^](D{ig(^T>T^ 0 cr— ^£g£V ^ 0 ) l^it)1-5„ " 
4-4 : ^^^T A (Climb) Kjfjs 

4-5 : (Finish) SbfN 

g/^Hf*6£;fv5£:. if— (Search) • 77^ (Align) •TT'c— ^-(Approach) l&ffrC 
-Y^. T^n— ^«J^Sj^5bLfc## (^^S22 : Yes) % ®jfctttmzXV&Wi. 
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■x^s22:No) , tkmvtc^b%mj3^x%m%mT~fz> 0 z.<Dm&&> * 

7 L y^ B S2l<?5*Q!ffld»feift?)3Ei-/j:ir-t-So 

^^S32) 0 rr-c, ^ta$^pgs<z>tt»-eisa ©ftffiS^riW-ssFiSoifiten 

SrflJ/frU ^£^step_min_z<n <step_max_z^fci-^^{^, ^PfeRTfi-e&S^J 

z z 

Sif-r^o %-&^W£f%&&fo'otim& ^^S32:Yes), »»^<T?W(feUe 
*3-rfdfelC x |®S tC^UTSkfcW'Ufcffillfli (align_distance) *-ejf£S}i-5T7'r>'tb 
^HfrL(^7 t y7 p S33) . #l^bJ;5iLTVN5^^^l^-t-5(^^^S34) 

V^^^{*(^^S35, S36:Yes) % ^O^O^n^h^^-CittfTT 0 
a-^»j^^fT-rS(^'7 i ^7 D S37) 0 — ^^y^'S35Jj:-C#ft'Br«*»a*S 
^V^#^^^^^S31(D^a^M^,^y^S36^TT^Vib^«^Jb-C 

x^5/^s22^*5{-t-5l@^#l^«)^^ov>-cmKi-So K©#P*«if^r±, 
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-hXh% 0 SXT<Dmm\Z^^X\^ m^Stt^^^B^^^step-O 

z z 

z 

z 

(^^S42:Yes)fi, ^iJ]{^(3) Sr^frf 5 (^5/7°S43) 0 ^1?— 

^LT. ^X^^S35t|D^, ^«3f^ (3) «^U7 v cb(^7 i 5/? D S44: Yes), If 
tfc^y:^ lOMS^^agi-o Pg&&&im£tlt£frotcWr£- (^^^S44 : No 

-T£5^ 09*. tf, ^^»3 c t 1 o^v^B(D^i2iL/ / R^^Bi8ii^s^i- 

12 

©i^, n^Vh^g{4, TO^(step-2)(D£^®182^Sf^-C#TV^ 0 ~T\ @ 
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f 5o #P^»f^(l)"Cf±, ^?t©S(step-0)©|§ffil81d^^C<7)S(step-l)©Kffi 

CD BITOT 

5-2 : #cgfc (step-1) ©Bgffil82<Z>:*:#&;&5+#ti::*:#V > 

5-3 : ^vlsh^^FE d^bSJEl21L/'R<D#S^-C<DEEfitfrontjc^tg^$tV7 v C# 

i 

P£^e — K it 5fM'^P<— *front_x_Umit £ (9 v n 

^it^^J^^^^^— ^back_x_limitJ;«9^:#V > 

3EHUSfe©(step-2)oK®183*«S!|-e#*i«^, }fcS(step-l)tf»gigl8 
20#^^{C*5{t5i«$zltHtC^(Dm(step-2)<Dj@®183<D#m^lC43tt^$ 
z2<DM(z2-zl)/5^. ^feS (step-1) ©8S®182^»e>-?:<D^©g!:(step-2)«)fi§Bl8 

<D$k (step-2) Ogg® 183dS38«|-e#/j;V 3ffi©#IW1ft#ilii$S:lx J »t><0 

0 

e>*f J ry7*184&jC&^ifr&lZ.te> (step-1) <Dj@ffil8207n^^5/^FE K: 

1 

LT\ ^O^^AKf^fd-C, ^<D^ (step-2) ^ffil83<^7t2>hai^FE |C77 

2 

2 

/^y^^-^BEtcT^^-r-s^-e^^T-rAt&f^ti-So 

=3r^2/^184^/h$v>^g-(C«, ^&Ogk(step-0)©Kffil81<Z>^!y^3iy 

v?BE ^©<Oggffil82tC*5»tS7n^h3:yt?FE ^{5}^— gci-^OiL-T, Mtl/ 
0 1 
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O 1 

1 

i 

(step-l) (D|@ffi^»'J-e#5#'a'^M-r§i^^- T? t5o ^^Ltf 2S#=ff n^Vh^ 

-c£K;&^fc3o 

step-o) \z&rtz>$&mfrhyK.wz (step-i) oei®£m^-f&!$i-5;L^T^i\ 2©$fc 

(step-2) X\tth£X±9c<Dm (step-m) <^©<D£@®^i^ Bl^*^#^t()^ 

^si2, 3^ov^t£i3i!-r5o 3fei* * 2^3fe<^^ (step-2) <D&m&mffi*j&tz»&\z. 

If— ^»f£(4)Sr^fr?-5 C*-7^X7°S5 Do ^.(D^—^m^ (4) -C}*, JW'tf-lf 

v ? 3^>^Al{Cj;«92^^D(D^(step-2)^i^®^^-^f^-r5o wtf>U— ^ftf^ 
(4)te, 2^3feO^ (step-2) ±^^^^543^^ tl^: 

^rb-C, ^^AK)f^ (2) ^^1-^(^^352)0 ^-fAKjf£(2)te, 

f^(i)iP#coifa^-e^o -cD#^t-fev^. ^^Atbm^t5isgi££3£5: 

ff^9<5SH1^>#l9#;tte, H3C<#;I8: (step-l) <Dgg®<^£n ^i^Jl^rrSri: 

z 

^-C^So (step-l) ^^(step-0)<D|^®J:i9^FP^W^Htl^ 

^rb-C, ^7-f^»)f^/5^5bbfc^(^^S53:Yes), ^^S51^T1^— 
^Ub&m, 2lfc5fcC9|Sgl3: (step-2) ^t±l$ix-CV^^(^5/7 P S54:Yes), 
^O^^LTjg^fL (step-l = step-2) (^5/7"S56) % ^x^^S51*^<D^iS 
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^^mmt^xmrn-r^ 

mXtbZ>&, Wm*S&fanmft<DgL (step-m) fre^igffi&IBfflfcNfefcLTV 
^UT, *9>fASbfls (3) (^^862)0 *7"fAIbfls(3) ^C*3V^ 

i i— 1 

f^o) iz.m^x&wiirz>mm<Dmm'Z. m>te<D&£vmmmzm&mizxm$m& 

<D7 t —#X$>Z> 0 

64) > m+n>k-C^>tb^ C*^>':7 e S64 : Yes) ^ step-(k+l)~~ step-(n+m)£\ step-1— 
step-(m+n-k)(n=m)^b-C, 3£0fU(^y^S66) , ;*xy^S6 ld>b<£>#!:«£rilfe 
•Jig-*", — m<0^fcixtf (^7 l y^S64:No)^^!tii-5*Mfei:>fe«Bffi* s 
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step_min_z : ^&<Dl£^J£Ol£icDif5£<D^ ) ©JW'^fl&ft/.HK 

ad (align_distance) : T^^{ie^fe{t5^n^h3i5,e/FEi:n7^Vh^fii:<^^0 

ax(approach_x) :T^o— ^{ig{r*5(t?)^D^hrc^v?FE^n^Vh^g^^FB^ 

fixmtjcJimitrl^SiC&itS^n^ha^ 
(minimal x-value) 

backjUinut:B§ffite:fcfrS^^ 
maximal x-value) 

backjt_desired:^V^^BEt&;£l21<D^ 
value) 

m«9±(f^stepjnin_z<J;i9/J^V^^tt©M^fcS (RlPSfc) t 

atja**J&V^«)^U stepjnin_zJ:l9^:#VN^(C«, |^#&^ RTi^J^rrSfc 
align.distanceii, T^^f^^S^^lCO^^ffl-f-^^— ^"Cfc"?^ PtSSr 
Z%-mhft*ft5m&fctiim£hZ>o mm^, approach_xt>, r^a— ^» 

front.xJimit&t^ackjUimitte, n^>l^gaSfglifcL-CV^i&ia^ oaKs/MIS© 

;ft^MJ:S>/h$vv#^c:te. *©Rgffite5£ttT*t;fcv\ Xf*^»j-C#fc£LTt>#; 
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(Dg-feMft&JFffiX'hZtyimZtlZo mmm^^^X, front.x_limitS.t5 

back_x_desiredte, ^^Oj@BSlc43^^^^^^BElC^UTP^h^g^r7 

35B{C^i-J:9iC N il^?, S5^§£"C_&;tXfi % back_x_desiredte:, /<y?^yi?BE£.y 
^HifeO?F^^*3V>T^. /^^^^v?BE^^15mm^fij(^'e:g^'feo-C 

IH36,£.t/__]37te, 035^^^^— ^^ffiUTH^^ci^Vh^a^ 

Hff ©ES^HNo. 1) , ^^^S33(DT^^ltlf^^tTCD#^-(No. 2) , Xrx/S3 

^(No. 4),^^X7 S S51<D1J— (4) H^TO^^- (No. 5) , ^7 t r/7 p S52©^ 

^Aif)^(2)||fi ; (D^T-(No. 6), ^5/^ P S52C0^7-r^»)^(2)cDM#-e^o-C 
^^<Dj^®^^y^^BE(CT7^^U-CV^#.-^(No. 7)^^^351(^-9— 

(4) H^o^T- (No. 8) , • • • Sr^U -y—J-mVF (4) «rLT3c«©«HBflMli 

ai$tt^^ofc#^ (no. 1 7) ^, m&g-&m¥mT (7^> a ) m^n^>x^ > 

3#^(No. 18)^-T„ 

. l,No. 4, No. 7, No. 10, No. 13, No. 16) , ^^A»!jf^(T7^S!)^^tf) 
$:I!)IL(No. 5, No. 6, No. 8, No. 9, No. 11, No. 12, No. 14, No. 15), 

^<Dmm^mm^ti^<^tc.m^xy^y^mi¥^n<>x(-No. isk 
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BCD ^^(Dm^^-tMXh^o H38(C*5Vn-C, 191-e^i-<£>f3\ f£® (z=0)frb 

19 i<Dmm TMfrh±.m\z. : &wi~fz>Wr&\^\ ^xmw-rz> 0 ^191 ^nvmm 

%m&x&ox ^£zo<D&mm%&mvx\<^bvTc.m&, zi-zo=3o>o 
x^<Dx^wt<D^.ni9i^<D^mnmm\^t^m^ z2-zi=-3o<o-c&3 

ront_x_limitS.T^back_x_limit«. PifvhM^UXhoX, U7$yK^W:?>&&121j)m 

38^-ri5^> mi®i9ifrbte&m^tcytnx&^xh&m^mbmm^%zbz 
v?$ybmm<v&j&<Dm&$:^-fmx&% 0 gi39^*5v^-c, i92-w-r©te, 

z=0)^<^I§5£;^zl=-30^fc3|M]^£^U ^-m, a^Vh^e^^(stepl 
)tft3Pa£|U92cD|ft®T«^^^ DUIF15192 

z2=0Xfo&i§&XfooX^ m^zOtD^mm^WjLrX^^bVtim^-. zl-z 

0 = -30 < OXhZ>(DX¥K.$k<D mn 1 9 2^<D&m-ZWQZ>WitfFhm $tU z2-zl = 3 

o > oxhz><Dxvm 1 92frtbik(Dmi&^<n&W} n&zwiitx&z tm m-tz-tft 
x%z> 0 l^o-c, &i$i9ibmm. zL<DWft\^fc^x?74J>mm^^x, ±& 
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o 

mm - mmx&£\ ^-a-e&o -ot>, ^msi • fmbT&spts wsr&^jjb ur 



I 

* 

WO 2005/087452 38 PCT/JP2005/004838 



&bVX<D^T^¥i/3^i/^TJ>>(Stereo Vision System) lb, 3^.jt(D^Mrf-~ & 

m^m^2ait. ^<D^mmm^nmmz.\^nn<D^—^n\z.^x^ 3 

m<Dm$Lmi$M:seed region) \ZMVX, m&i&Mtm— ¥®t-J!-r3$l#£Jl|I#C 
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A^^5^U^^llR/L*#^St*^,^^tfl^U<gRlflJ^yh3fe^ 
*3tt3*&S£"f5o S^rt^^J^tftT^lBjCDiij^lJ (image row:^— i?uV)K& 
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)<D&mmmz#i^xm~ ¥-nfcm-tz>k&%£te? t — ?MA*hf&&&iftb&fr 

£T , spffi<Dfltefc*«ifc(£n^ ffi^Cseed region) £V^o 
over-segmentation3^fiunder-segmentation^^ft><7)^M^ s ^C"CU^5o -t£."C» ^ 



• 
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T3o 

o Bft&ltiMUne Extraction)2af2, ±3£Ut.fc5fc x XT\'$r\fi?al'i'a J ?J»lfrh 

^[ZSt^XMS^^i^^^yT^^^T/^]}^ (Adaptive Line Fitting) £ 
#A1-5o Adaptive Line Fitting^, mft$HiiU2alZte\, ^X , 3fe-f it%l&J±^^M 

m&m.m vxttfr\z.m 1 (DWdt^xvuft^ ttiu* i-tt misw-m 
M.<D^(D&^^>^^m^xm— ^m^jffevx^^fr&fc&tt&m 

(zig-zag-shape) {zmXfe&Zm&ia^ ttft{zmQ&foZ>kL,Xy*~?£M 

^xm^m\^u^(Dmm^oT;v^x^^.m-r^(Db-r^ 0 

M.M (P [0 • • • n-1] ) tVXmiHi-^ &V?S8l) o ZrVX^cD'r-fMmP [0- • • 



» 
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n-1] \Z.^tl^l/7;V^ntmm\^Mcm&-&W£^-7^W. {>&mMvhm min_n 
±<9#V (^7^^382) „ »n^^^^/J^ m in_nJ:«9^^ 

V^^(S82:YES) t-«, ^ttS^m^bT^^m^bT^ll^Ti-So 

[0- • -n-l](D-^<DMP[0]tftil^-<D^P[n-l]^^^(^)Ll^l^ 

±%\,^-?&$:mn&brkk^xmmu ^omfitdist^mmi-sc^^sss^ 

ft^l^ildist^x-^^^*'J^il^ttmax.dii9^V^«'a'(C«(S84:YES) , T' 
—?£M'r-?&MPlQ'' -n-1]^ g^(^-«^)brk^T2o^x-^^Pp[0- • 
•brkl&tfPCbrk- • -n-ll^fij-rS (^^■7 B S88) 0 
— ^^^#JoSijii:max_dJ;f9/J^V^'^^^(S84:NO 

line^5fc£> (^y^S85) . ^(D^@^line^^-^ : 9-L2^2(D^^tL."C^^t" 
So ^LT, x— ^pP[0---n-l]^-(D^L2^*|-UT^^-r5>Zig-Zag-Shape 
-e&£^5;^£P-<5 C*^.y:/S86) . Zig-Zag-Shape-C&V (S86 : NO) , # 

^X5/^S86 ^*3V ^ X*^y?*S85X^lbtlj&ft&Zig-Zag-ShapeX&%m 
$r£;ft,7i*§^(S86:YESh ±^0^5/^384^ 1^1^, ^y7°S88^it^, Xf 

[0- • •brk]S.tl ? P[brk- • -n-l]^SiJ-t-§ 0 ^<DXx5/^S88fC-C2o<Z>x— ?£M& 
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<Dkvx®muta>K m%-& s 7 t .—?j§mpio- • -n-i]^^ ^sc^^ioscr 

m^., ^^7 0 S85{^*D{t§S/J^^l-«t^^^^^"& (Least-Squares Line 
Fitting) \^\^Xmm-tZ> 0 fc5ni©f^MP[0" -n-l]/^^ x-^&S^ 

mmi)xm-t 0 
mil 

xcosa + ysiaa + d = 0 -(l) 



G6fc2] 



X-oX^tbhiVbo _Lffi5£ (2) £#yN-1-5 aRTfdte^ "f-*MMP<D¥&}RXfift 

[|£3] 



= X (*/ cos a + yi sin a + <f ) 2 



a = - tan 
2 




"(3) 





■ 
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^5/7°S86^fcnt5^W^ (Zig-Zag-Shape) WJ^&^Ol ^Tt&^-f- 
So ^<DZig-Zag-ShapeWJ-?te. fo5n{@<D-r— ^8£P[0- • -n-l]^E^Line( a , 
dk xcosa +ycosa +d=Otf-$Z.btltlb%. ^Of-^SfPCO'-n-l]^, 04 

tt^it 5* j^LT ^tx3*£ Id fit, zig-zag-shape^fcS Wf T 5- £ 

0 g|45A<E>J&£*K:tt % 7*— ^^P[0-«-n-l]tCj;t)J;<7>fyhi-«iffiliLineS:*«) 

^^P[i]Sr5>S"J-t-5j^S36 s *>«o 046}*, Zig-Zag-ShapeWJ^ 

£1\ 7*— ^PP[0---n-l]iHG^Line(a,d, a )^SrA^7^S(^7 t y^S90). 

«97C#V^^^J^f-f-5 0 -(D^W^ cr £Wftth_a ±^/J^Vv|§^(;*7WS9 

1 :No) ^^CD^K/jN^^^^^iSm^abScD^^^lHlM-t-Sfcfe, 

{RliiCfc^^^sing(sdist(P[0]))^J;o-C^J^U. ^(D^M^val tdftAi-^^-val h 

0 0 

S92) c sign(x){3. xCDM<^^-^( + Xtt-)^5gi-K^X-fcD. sdist(i)^, P[i] 

.xcos a + P[i].ycos a +dtL-Xf J rW^^lt'W.MiLinel l Z.id\ ^T, i# S CDx — IE 
AOEEHt&^-f „ 1-&fc>^ Val T^-^^PCOl^iiiaLine^^^^IiJ^^d- 

0 

^^^^O^^^MSi^T 5 — ^n£9/h$Vv|§^ (^7^^394:^3) , ^(DtK 
fc5/^£rsing(sdist(P[i]))^J:oT¥JWU icD^m^val^ftA-TS (^^5/7°S95) 0 
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0 0 

kifiMteZ&&frTy~7S96: NO), val UlvalSrttAU 5tJ*/SC*^^©*!>^HItt 

o 

counts lSrftAb(*^s>:/S98) , ^^^^v^o^;^^Hfti^^y^^M-T 

(Xry^S100)^^:7 B S94a>bO#!;StfcM5o 

o 

!>^Miicount^lO-f'>'^y^h-r^(^5/7 0 S97) 0 JEf^ 5Utt^^^O*^^ 

5** J RI^V(^S'^ , S99)» ^V^^^tCfi (^"^y^S99: YES), Zig-Zag-Shapet 
¥UWrU TRUE&m^L-C&LS&H-T-f-So — £\ it^^V^y^^MIcount 
tfSft/hOx r — ^&i£min_cJ;9/h£V (^jy^S99 : NO) , TsryZfS, 100 

&nlzm&tZ>i£-em-f. *!?Vhii>=n^ofc^f, FALSER m^LT^Jg 

r<Di5^^if^BWI^S^J;o-C, ni@<Dx-^^pP[0- • -n-l]^E0Line( 
a ,d) :xcos a +ycos a +d=Off-$-z_t>fltzb%, iCf- ^ M^M^WMhrneizM 

^y7 p Sl00*"eo«3.ffittl2l47©J;5^^S"t _ 5wi: l t>'5rt6-C*>5o 

£fc, £<D£5fcZig-Zag-Shape#JSiI*Q.3ffitt, SUMSOSw&bf KP*7"T!fT5 
T?fc5o 048(^-ri5t-> Zig-Zag-ShapeWJ*Q;3!FB20tt: % n|S©f-^MP[0 
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0 

WCVal=Val <Dm&\Z. %V>\^*4>W*l'YttWfcM33V'y?2Ab, 

0 

^&minj5fc^tfc^3JttfelflS25££^ri-3„ 

JttSElfB23l4 % -x— i^^PMt^— ^^P[i-l]<^_hlBjEAO^F-^-^Jtifeb, PIDMF 
^^^(-^[>>-^24<7)^^>'Mecount^^yp<>'hi-^f^t±J^U, IE 

{ficount^A^V^^iCte, y 5 — ^/&i¥P[0- • -n-1] ^J^VX^^b^-Tim 
m40K7jk-rffii&$j:Mn (Region Growing) 2b fcOVvOfcBHi^o IS*£i£3g 

$B2b«, mfr&mu2a^£^xnb}vtcmftm$:x£)bu ^wbomtttrfti 

tfb(D¥mzmL,X^Zfa&&m<D¥m^<D^X\-Zlt> (Plane Fitting) ICj^JfilWrU 

^^^-5^S¥^^^^^m<^¥® (¥®^) t-^-SI-r^o ««fe©spii»!: 

O-Cfct), i03#©*ft#a*££*l^®#^#»(seed region) £V^5» ^bT, 
spS^CO^Tftfe (Plane Fitting) izXVmWrU »&^3l&##raC¥ffifc"£*h/* 



-1 



r 
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o 

©3^:<D^^^^^^bxil^tLfcii-2) 0 ^ttP>3^0H^32a— 32ca>b&3 
5?tSP"^fc, 5pffiP'«rH*fi-« 0 ^4i4r»«93gi-ifct-J:t), PfS3l<z>2aB(E> 

Fitting) „ rtuSr^fflUT^^a?T)l^-f~S (Selection of seed region) » ffii^S 
d>^i££&:*UTVK^^&^S (Region growing) x RUnhthtc^m^fU^: 
^i^M^^V^O&^^V^TIIS^ffil^^^atPost processing) {doV % ~C 

3*7C^Krt<O^P«:P= (x, y, zMn.W^Ftu ^ffiO#m^te^ife§£-^r- 

ill 

/Wi(nx, ny, nz) fc#£©^&dfc±oTTiE5t(4)^&*t3„ 
[«4] 



Arn x + ^ + zn z + d = 0 —(4) 
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(5) {z^-rim^Mz-r^mtvx^^t^x^ 

[«5] 

/if(n,<*) = £(pfn + <ar) 2 - ...(5) 

I 

#:il#?f4n=m/ " m ■ , d=-l/ * m • tbX**5 0 ■ • ■ 14, 

mtt, tT^^fcJ:oT3gS:— »C*^&Sr#<^9^— A'OjSfeMU (Cramer's rule)2M£ 

ffib-CTta (6-1) <d \zM% KMhf^WkWy*7-J±.(F>mx&Z> a 

A m = b ...(6_2) 

A = ZPiPf» b=£p. (6-2) - 

<D2<XD^—^h(l^.^r—^h:W-^. 2#Z^— : &W0 E (p) , E (pp )/5>gE£p 

T 

XhV, :tl^ffllt, TIE (7) \Z^rt£?\ZA, b£3E*r*-5r.£j&S-C#, nl@<7>^ 

mi] 



A<-A + n£(pp T ) f b<-b + nE(p) ...(j) 

—^^CP^Apg^^^o^^^V ^^-t-5p®^S^(D2m¥^^ (RMS 
(root mean square) residual) (J^T, rmstVvpo ) £rTlE^(8) ICiO^tfj't'Sit/^ 



t 

WO 2005/087452 49 PCT/JP2005/004838 



rms 



(8) 



1 n 

mmmiseed region) %&m^%XW:RTf&m.mfrb^&fc±1rZ>bmz 
H#(l ,1,1 )-?fco-C, 2V^CD^(l , 1 ), (1,1) ^ttSMl^g^JiiET 1 - 

1 2 3 1 2 2 3 

H^rt^^ttSH^^g^i-^r^y^ (index) ^#U-C*3«9, ^J^^^T^fBjO 
V^/^/^SrJtgrrSo r<D^(C^bfc^(^y^°S102:YES), _hfB^(7 

ttS^fflU-C_hlE^:(8)^i-5pffi^S5^02m 5 P*&ift3l(l ,1.1 ) £rf+#1-5 

12 3 

9^8103) „ -^LT. ^©¥ffi^SS:©2l?^lrms (1 , 1 , l)&m*.t£lcm 

12 3 

rms 

5(^5/7°S104) o 0f^<^Pfeth lJ;0^:#V^»^|ctt % Wtfc^s^SlOllJlM 

rms 

1 4 

^^Lfc*^(^y^S106:YES) N -t(05pffi^g^«)2^5p*&ISIIrms(l )& 
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fflU Z-iltmfeOMmth 2±9/h£V^^&^U^X^S107) , /h£V^ 

rms 

Z> 0 ^LT, <ft^»fc-&*H5IHJKW*<5!tofetfra-(^y ^"S102:NO), 4"* 

rr-e, ^WJ^*5VNTf*. fi#$tt&&XU 3oco^^isj— 5p®ic«i-«** 



¥*»gbms#0fi&&BM* (thjms) *lT-fc5t^tu(D^^©t^ (P) StIrI— ¥■ 

^i^ttK^a-rS^, a S Oj£i (tefr) <Dffi& (end point) £^®P£<D{lggtD<D 

¥S^S^2^TO^Mrms (La) {^itUTlSlSlBOjim-b^^^^^P®^© 
^02f^i^l^rms(Lb)tf);fr^#V\, -f-f^ib, 051A<£><fc5^ iUflLa^ 
SPir^^M-T-S^f*. ¥®^a^2^5jZ^Hrms^it^fi<J/J^</'l'X<^j^ 
#T»fc5»^as#V^t£>tfU EI51B<E>«fc5fc4§£\ ¥®^"^^2^5p*&m^rm 



t 
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o 

r 

£fc, ±3£<D£gis£«<Dil^;fr#rte, m52<D£?\Zhm%L1-Z>Z.btfX*£Z> 0 overlapO 

j k j k 

fitPlane(l , l , l _LfE5£(4) — (7) {£j:<9Am=b<^^#)^P®/^— d£ 

12 3 

ff-mU _hfS^:(8)tCj;«9^m§^fcA, b^iO. BJI^IvM ,1,1 %¥mz7^y 

12 3 

rmsd , 1 , Of*. ±IS5£(6)£MbT3^<Dmil^Tl£:feV>-T, ¥®^5£tf>2 

12 3 

^ 5 Pi^^Mrms(D'fa:^^tB-t-S||^:-e$>5o *fc, removed ,1,1 linesCi], lines 

12 3 

[i+l]l , lines[i+2]d^^a^^i-5tL-Cjl^^c^tV^fUii:^l , 1 , 1 

2 12 3 

bfi. ±IE^;(6-l)^i-^:^tVfT^JS.U ; -<^h/V-efc§. £fc, add(A, b, Of*. 
±te^(8) \Z£<9, AkbfcmMlme^^r—^b&MZ-ZmgtiXhZo Solve(A, b)te, 
Am=b^fci-m^fe, ife^^-C?)^^®'^-^!!, dSrfr^-TSo 
select(open){£ N ^Jx.^ft?0OlO^^\ openO^^teSl-lO^^W^r-Sril^ 

•tz>mwt.xhz> 0 *fc, indexd m^^Jxfitf fc&ttsi o^^v^^is-rn 

1 1 
3&-C£>3 0 *fc, neighbor(index)te\ ^X.^^^l-fVxV^t-^bfc-l'^X^^, 

#1 X. fcf {index- 1, index+l^Mi-M^-efe-So 

Mtili-Z&kmiPost processing) ^.(Dn^M^^^MX^ M?tlt±i&<D 



> 



t 
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zi>(D<Dmzmm£¥m&&ibz>zb&x%z> 0 mfc"?—? &*mw&, sw®*- 
mw&MuTzvtcfficDffcm&jF-tmim, m54B«, £&#£x, aw^y, 

d^^ttttiMS^TI^I— ^m^^^h^fl^^—^ MM^hmM^^Ui. 
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tc, lfa{ti<Dj$ l : g (correct extraction(horizontal)) tt % ff^^\<D"f—^M\Z. 

ttttjCD^^CMii:) (correct extraction(vertical)) fe, FWfaiD'f— r5^!|£M£oV>T 
(Dt^m<D^^ir 0 Hfc. No. 1— No. 5W\ ±5£WciZig-Zag-Shape«J&!!S£ 

mAtix^^^^m^nmm^^^iii^m^w. no. 6«u 

Zig-Zag-Shape^JSIJMa^tTo/c^^?f0f!l^*5{t?)¥®^m^i*^)^#^-ro 

®^ffiUfcf£^(ttiS^)£^i- 0 IH55BK:^1-J;5K, ^*(D^^*3V^^, ^ 

(Line Fitting) l£^T*£^<Dfcfe</D|Ii<fji>-^~ ^max_d<gr:*:t<-t-5 ( 
max_d = 25, 30) <W&tt!^T;^ Illffeax_d/h£<i-5(max_d=10, I5)£1&m^ 

^i^jti-5 0 ztbizttLrX. *mm<D£?\^ ^■^^mmmm^mx-r^ticx 

itlX&<9 N ^{i:niax_d^/jN^<i-5^, y-TX(D^iP^§:{t-C^tB^#<^o-Cb*9 

max_d^^:#<LT%, H^y-rxo^w^^^v ^^(Dm^x^x^m^mia^ 

ivX\,^Z.ktft>fr& 0 

*£&#LT¥ffi&ttJl-5^3 s T^5o flifcfif, HI ll^Hll 2^1-^9 Mtb 



♦ 
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^m.mxh^tvxm^^-mir^ kvtc<dx% M^^ts rati m>tbM$k 
mm-rzz.bh~*smxfoz> 0 
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[3] 3. ±M&m^$kb\.xmn*mx.. ^mm^x^m nifexhz 
[4] 4. ±mvg&mm^m-±. 

mz^mi-^bizxy)m^vtcm^m^mm^±m^mmb^xmji-r^m 
zb%w&bttm&(DmmmimmM<Du^yhmm 0 
mmu z<Dmm&^x&%mmmm&±mm&mvg&mmb^xmtiu 

[6] 6. ±j§mmmmL^Wite. iM^mmm^m^mm^^m^^mwmm.^ 
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[9] 9. ±mmmtn ±^mmm^&^mmtm^titcmm<DM'C?^-r0 

[io] io. ±aa#flS/jSiflMiH:, ±»7ovh^^xrj^<^aiy5?K:i*^feiaijK^i" 

ii^#mti-^ft*(Df5H^9^|E^n^Vh^« 0 
[11] 11. ±|EigffiW#Mu mSi^S^S^^-r-S^OS^^n^W^^i-S 
-i:^#mi:i-5lf^<D$5IS^7^IE«con^VhSe o 

[12] i2. ±fe»gWMM^ara, ±is^mt^^#¥®o^#£^ffibT#^£ 

[13] i3. ±tttmmmzm^mm£ntc^<Dmm^m-rz>\!h& 
mmmxtbz 

[u] i4. ±m^mm, ±m^mmm^m^mm^tcw-m<Dmm^mir^&^ 
mmi&x$>% 

ztzmkb-tztn &<of& mm 1 2^iE«on#5/h^g 0 
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^b&wmbir%m$:<Dm,mmi7mtE,ifc<Du^ybmw:o 
19. ±!EPgi&#iM^i£te, m^WiM^b^mm^mu ^mmn^b 

^b^mbir^m^<Dm l mM3mm.<Du^ybmMo 
zb&wm.birzm&<D^mmi9mmm<Dti^ybmWo 

21. _ki2Pgl£#MJ$]#Ig:«\ m£<D&mm^tert%^yy-^y*J\zttm\^ttm'& 

zb*w&birzm&<D&mm3mmm<Dti^ybmw:o 



t 
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[23] 23. ±M'*7*— ±mmn<D&±lfM£JUZ&Tl? ffiSfc3E<5#«fc^§ix5 

[24] 24. Pg^^^«jf^t|^t)§l!]f^^-e±|B^^— ^(D^m^'S.ir^^—^m 

[25] 25. ±fB¥®^m^«, 3^5c^^-t?|^-5p®±^$>S^4t^^^£g^-^ 

[26] 26. ±IE^ttm#^». ifESgglx-^^P^cDEggKcX^^— 

[27] 27. ±§E^ttffi^^. ±ibi^— ¥m±iz&%bmfe£nzmm-T~?M.mfatb 

zb%W{§kb^znM<7ymmn25mzM<Du7$ybmw<> 

[28] 28. ±iE^ttm^«, ±mm~¥m±\z&%bmfe£ft%m£y*~?Mmfat> 
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[34] 



[29] 29. ±fB¥®«$i6l6^g!tt, P3— ^¥®^JRi-§tti^$ti^)l^_bcD^^ 



I. 



[30] 30. ifBHff^S^S-rsm^-^^^^V^, S^Htf^¥®^Eg 
[31] 31. Jifa¥®^t£^#^tt,^^i«9S*52p®i_hfB^¥®i:^mM^ 



.t§r#^i:i-Sff*C»t5H^25^iE«con^^h^g 0 



[32] 32. &m^mz£y&m^mt£v^yhmu<Dmftmffl^mz.m^x, 
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Close gaps : 






for each triple of consecutive points do 
if distance first to last point < d m then 

remove middle point 
end 

until no more changes 




Fit line segments : 






for each triple of consecutive points do 

compute least-squares line fit with error 

end 

for each fitted line, sorted by error do 
if error < e fit then 
remove middle point 
update first and last point 
re-compute fitted lines for points 
preceding and following line 

end 

until no more changes 
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Input: pts : vector of points, n : number of points, Ot,d, a : parameters and std-dev of fitted line. 
Output : true if curve contains a zig-zag- shape, false otherwise. 
Sequence : 

if a > thresho then 

vaiO =pts[0).x* cos Of +;p&[0].y*sin cr+tf 
for i- J to (n-I) do 



vai =pts[i].x* cos a + pts[i].y* sin ce+d 
if (vai* va/0 <=0)then 

valO = vai 

count = 1 

else 

count = count + ) 

if (count min-points-for-zig-zag-shape) then 
return true 



endif 



endif 



endfor 



endif 



return false 
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Algorithm FindSeedRegion 

Input: linesfij: vector of lines for esch image row (or column) /, 

n : number of image rows (or columns) 
Output : set of lines (seed region) or empty set (no seed found). 
Sequence : 
for i = Qto (n-3)fo 

for lj in linesfij do 

for 1 2 in lines[i+l ] do 

for 1 3 in lines[i+2] do 

if overlap(l l9 l 2 ) and overlap(l 2 ,l 3 ) then 
(n,d)=fitPlane(l ! ,l 2 ,l 3 ) 
if rms(l ) ,1 2 ,1 3 ) <threshl nns then 
seed={l n l 2 ,l 3 } 
remove(l } J 2 ,l 3 ) 
return seed 
endif 

endif 
endfor 
endfor 
endfor 

endfor 
return {} 
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Algorithm RegionGrowing 

Input: region : set of lines as seed region, 

linesfi] ; vector of lines for esch image row (or column) /, 
n : number of image rows (or columns) 
Sequence : 

A=0,b=0 

for 1 in region do (A 7 b) = add(A,b,l) endfor 
fad) = solve(A,b) 
open = region 
while not empty(open) do 

/ ; = select( open), open = open - {1 } } 
for / in neigbbor(index(l f )) do 
for 1 2 in lines[i] do 

if overlap(l n l 2 ) and rms(l 2 ) <thresh2 jms then 
region = region + {1 2 }, 
(A,b) = add(A,b s 1 2 ) fad) - solve(A,b) 
open = open + {1 2 }, 
remove(l 2 ) 
endif 
endfor 
endfor 

endfor 

plane = fad,A,b,region} 
planes = planes + {plane} 
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